rodAndBarDetection version 1.3.0 : 新的定位盘中心测量功能
This commit is contained in:
parent
5378038e3b
commit
1ed1e1242d
@ -13,6 +13,7 @@
|
||||
#include <Windows.h>
|
||||
#include <limits>
|
||||
|
||||
#include <cmath>
|
||||
typedef struct
|
||||
{
|
||||
int r;
|
||||
@ -31,6 +32,34 @@ typedef struct
|
||||
float b;
|
||||
} SPointXYZRGB;
|
||||
|
||||
|
||||
// 点乘 dot
|
||||
double _dotMultiply(const SVzNL3DPoint& a, const SVzNL3DPoint& b)
|
||||
{
|
||||
return (a.x * b.x + a.y * b.y + a.z * b.z);
|
||||
}
|
||||
|
||||
// 模长
|
||||
double _length(const SVzNL3DPoint& a)
|
||||
{
|
||||
return sqrt(a.x * a.x + a.y * a.y + a.z * a.z);
|
||||
}
|
||||
|
||||
// 计算两个向量夹角（返回 角度）
|
||||
float _computeVecAngle(const SVzNL3DPoint& a, const SVzNL3DPoint& b)
|
||||
{
|
||||
float l = _length(a) * _length(b);
|
||||
if (l < 1e-6f)
|
||||
return 0.0f; // 避免除零
|
||||
|
||||
float cosTheta = _dotMultiply(a, b) / l;
|
||||
cosTheta = cosTheta > 1.0 ? 1.0 : cosTheta;
|
||||
cosTheta = cosTheta < -1.0 ? -1.0 : cosTheta;
|
||||
float rad = acosf(cosTheta);
|
||||
float degree = rad * 180.0f / (float)M_PI;
|
||||
return degree;
|
||||
}
|
||||
|
||||
void wdReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector<std::vector< SVzNL3DPosition>>& scanData)
|
||||
{
|
||||
std::ifstream inputFile(fileName);
|
||||
@ -450,15 +479,15 @@ void _outputRGBDScan_RGBD_centerPose(
|
||||
SVzNL3DPosition* pt3D = &scanLines[line][i];
|
||||
|
||||
if (pt3D->nPointIdx == 2)
|
||||
{
|
||||
rgb = { 135,206,235 };
|
||||
size = 5;
|
||||
}
|
||||
else if (pt3D->nPointIdx == 3)
|
||||
{
|
||||
rgb = { 250, 0, 0 };
|
||||
size = 5;
|
||||
}
|
||||
//else if (pt3D->nPointIdx == 1)
|
||||
//{
|
||||
// rgb = { 0, 0, 250 };
|
||||
// size = 3;
|
||||
//}
|
||||
else //if (pt3D->nPointIdx == 0)
|
||||
{
|
||||
rgb = { 200, 200, 200 };
|
||||
@ -500,7 +529,7 @@ void _outputRGBDScan_RGBD_centerPose(
|
||||
|
||||
//输出法向
|
||||
size = 1;
|
||||
double len = 60;
|
||||
double len = 110;
|
||||
lineIdx = 0;
|
||||
{
|
||||
SVzNL3DPoint pt0 = { poseInfo.center.x, // - len * poseInfo.normalDir.x,
|
||||
@ -878,7 +907,7 @@ void _outputRGBDScan_RGBD_weldSeam(
|
||||
sw.close();
|
||||
}
|
||||
|
||||
#define SCREW_TEST_GROUP 7
|
||||
#define SCREW_TEST_GROUP 10
|
||||
void screwTest(void)
|
||||
{
|
||||
const char* dataPath[SCREW_TEST_GROUP] = {
|
||||
@ -889,17 +918,22 @@ void screwTest(void)
|
||||
"F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云4/位置2/", //4
|
||||
"F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云4/位置2未动螺杆拧进去100mm左右/", //5
|
||||
"F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云4/位置2向前100mm螺杆拧进去100mm左右/", //6
|
||||
"F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云20260621/另一根螺杆/", //7
|
||||
"F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云20260621/拍照点1/", //8
|
||||
"F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/螺杆点云20260621/拍照点1xyz移动10mm/", //9
|
||||
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[SCREW_TEST_GROUP] = {
|
||||
{1,4},{1,30},{1,11},
|
||||
{1,20}, {1,20}, {1,5}, {1,21}
|
||||
{1,20}, {1,20}, {1,5}, {1,21},
|
||||
{1,10}, {1,10}, {1,10}
|
||||
};
|
||||
|
||||
const char* ver = wd_rodAndBarDetectionVersion();
|
||||
printf("ver:%s\n", ver);
|
||||
|
||||
for (int grp = 3; grp < SCREW_TEST_GROUP; grp++)
|
||||
for (int grp = 9; grp < SCREW_TEST_GROUP; grp++)
|
||||
{
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
@ -908,6 +942,8 @@ void screwTest(void)
|
||||
|
||||
if(0 == grp)
|
||||
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
|
||||
else if( (grp>= 7)&&(grp <= 9))
|
||||
sprintf_s(_scan_file, "%sLaserline_%d.txt", dataPath[grp], fidx);
|
||||
else
|
||||
sprintf_s(_scan_file, "%s%d_LaserData_Jl26C299.txt", dataPath[grp], fidx);
|
||||
|
||||
@ -962,12 +998,23 @@ void screwTest(void)
|
||||
&errCode);
|
||||
|
||||
long t2 = (long)GetTickCount64();
|
||||
printf("%s: %d(ms), errCode=%d\n", _scan_file, (int)(t2 - t1), errCode);
|
||||
|
||||
//输出测试结果
|
||||
sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx);
|
||||
_outputRGBDScan_RGBD(_scan_file, scanLines, screwInfo);
|
||||
sprintf_s(_scan_file, "%sresult\\%d_screw_info.txt", dataPath[grp], fidx);
|
||||
_outputChanneltInfo(_scan_file, screwInfo);
|
||||
char _out_file[256];
|
||||
sprintf_s(_out_file, "%sresult\\%d_result.txt", dataPath[grp], fidx);
|
||||
_outputRGBDScan_RGBD(_out_file, scanLines, screwInfo);
|
||||
sprintf_s(_out_file, "%sresult\\%d_screw_info.txt", dataPath[grp], fidx);
|
||||
_outputChanneltInfo(_out_file, screwInfo);
|
||||
|
||||
if (screwInfo.size() > 0)
|
||||
{
|
||||
SVzNL3DPoint zAxis = { 0, 0, 1.0 };
|
||||
float angle = _computeVecAngle(screwInfo[0].axialDir, zAxis);
|
||||
printf("%s: %d(ms), errCode=%d, {%f, %f, %f}, 与Z轴夹角=%f \n", _scan_file, (int)(t2 - t1), errCode,
|
||||
(float)screwInfo[0].center.x, (float)screwInfo[0].center.y, (float)screwInfo[0].center.z, angle);
|
||||
}
|
||||
else
|
||||
printf("%s: %d(ms), errCode=%d\n", _scan_file, (int)(t2 - t1), errCode);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -975,7 +1022,7 @@ void screwTest(void)
|
||||
#define LOCATING_PALTE_TEST_GROUP 1
|
||||
void locatingPlateTest(void)
|
||||
{
|
||||
const char* dataPath[SCREW_TEST_GROUP] = {
|
||||
const char* dataPath[LOCATING_PALTE_TEST_GROUP] = {
|
||||
"F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/定位盘点云/", //0
|
||||
};
|
||||
|
||||
@ -1030,22 +1077,79 @@ void locatingPlateTest(void)
|
||||
}
|
||||
}
|
||||
|
||||
#define ROD_POSITION_TEST_GROUP 2
|
||||
void rodPositionTest(void)
|
||||
#define NEW_LOCATING_PALTE_TEST_GROUP 1
|
||||
void newLocatingPlateTest(void)
|
||||
{
|
||||
const char* dataPath[ROD_POSITION_TEST_GROUP] = {
|
||||
"F:/ShangGu/项目/冠钦项目/矩森棒材抓取/", //0
|
||||
"F:/ShangGu/项目/冠钦项目/胶布圆棒抓取/模拟测试数据/", //1
|
||||
const char* dataPath[NEW_LOCATING_PALTE_TEST_GROUP] = {
|
||||
"F:/ShangGu/项目/冠钦项目/螺杆测量/配天现场点云/定位盘点云20260621/", //0
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[ROD_POSITION_TEST_GROUP] = {
|
||||
{1,8}, {1,5}
|
||||
SVzNLRange fileIdx[NEW_LOCATING_PALTE_TEST_GROUP] = {
|
||||
{2,11},
|
||||
};
|
||||
|
||||
const char* ver = wd_rodAndBarDetectionVersion();
|
||||
printf("ver:%s\n", ver);
|
||||
|
||||
for (int grp = 1; grp < ROD_POSITION_TEST_GROUP; grp++)
|
||||
for (int grp = 0; grp < NEW_LOCATING_PALTE_TEST_GROUP; grp++)
|
||||
{
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
//fidx =2;
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
|
||||
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanLines;
|
||||
wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
|
||||
|
||||
//转成plyTxt格式
|
||||
//sprintf_s(_scan_file, "%s%d_ply_Hi229229.txt", dataPath[grp], fidx);
|
||||
//wdSavePlyTxt(_scan_file, scanLines);
|
||||
|
||||
long t1 = (long)GetTickCount64();//统计时间
|
||||
|
||||
SSG_cornerParam cornerParam;
|
||||
cornerParam.cornerTh = 45; //45度角
|
||||
cornerParam.scale = 5.0; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
|
||||
cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4;
|
||||
cornerParam.minEndingGap_z = 5.0;
|
||||
cornerParam.jumpCornerTh_1 = 15; //水平角度，小于此角度视为水平
|
||||
cornerParam.jumpCornerTh_2 = 60;
|
||||
|
||||
int errCode = 0;
|
||||
SSX_platePoseInfo centerPose = sx_getLocationPlatePose_new(
|
||||
scanLines,
|
||||
cornerParam,
|
||||
&errCode);
|
||||
|
||||
long t2 = (long)GetTickCount64();
|
||||
printf("%s: %d(ms),errCode=%d\n", _scan_file, (int)(t2 - t1), errCode);
|
||||
//输出测试结果
|
||||
sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx);
|
||||
_outputRGBDScan_RGBD_centerPose(_scan_file, scanLines, centerPose);
|
||||
sprintf_s(_scan_file, "%sresult\\%d_screw_info.txt", dataPath[grp], fidx);
|
||||
_outputPlatePiseInfo(_scan_file, centerPose);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#define ROD_POSITION_TEST_GROUP 3
|
||||
void rodPositionTest(void)
|
||||
{
|
||||
const char* dataPath[ROD_POSITION_TEST_GROUP] = {
|
||||
"F:/ShangGu/项目/冠钦项目/矩森棒材抓取/", //0
|
||||
"F:/ShangGu/项目/冠钦项目/胶布圆棒抓取/模拟测试数据/", //1
|
||||
"F:/ShangGu/项目/冠钦项目/胶布圆棒抓取/模拟测试数据2/", //1
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[ROD_POSITION_TEST_GROUP] = {
|
||||
{1,8}, {1,5},{1,11}
|
||||
};
|
||||
|
||||
const char* ver = wd_rodAndBarDetectionVersion();
|
||||
printf("ver:%s\n", ver);
|
||||
|
||||
for (int grp = 2; grp < ROD_POSITION_TEST_GROUP; grp++)
|
||||
{
|
||||
SSG_planeCalibPara poseCalibPara;
|
||||
//初始化成单位阵
|
||||
@ -1069,7 +1173,7 @@ void rodPositionTest(void)
|
||||
{
|
||||
//fidx =1;
|
||||
char _scan_file[256];
|
||||
if(1 == grp)
|
||||
if( (1 == grp) ||(2 == grp))
|
||||
sprintf_s(_scan_file, "%s%d_LaserData_Hi229156.txt", dataPath[grp], fidx);
|
||||
else
|
||||
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
|
||||
@ -1239,21 +1343,35 @@ void rodWeldSeamPosition_test(void)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
typedef enum
|
||||
{
|
||||
keSG_测试_配天螺杆定位 = 0,
|
||||
keSG_测试_配天定位盘定位,
|
||||
keSG_测试_配天新定位盘定位,
|
||||
keSG_测试_棒材抓取,
|
||||
keSG_测试_筑裕钢筋焊缝定位,
|
||||
} ESG_testMode;
|
||||
|
||||
int main()
|
||||
{
|
||||
#if 1 //螺杆定位测试
|
||||
#if 1
|
||||
screwTest();
|
||||
#else
|
||||
locatingPlateTest();
|
||||
#endif
|
||||
#else //棒材抓取定位测试
|
||||
#if 1
|
||||
rodPositionTest();
|
||||
#else
|
||||
rodWeldSeamPosition_test();
|
||||
#endif
|
||||
#endif
|
||||
//ESG_testMode testMode = keSG_测试_配天螺杆定位;
|
||||
//ESG_testMode testMode = keSG_测试_配天定位盘定位;
|
||||
ESG_testMode testMode = keSG_测试_配天新定位盘定位;
|
||||
//ESG_testMode testMode = keSG_测试_棒材抓取;
|
||||
//ESG_testMode testMode = keSG_测试_筑裕钢筋焊缝定位;
|
||||
|
||||
if(keSG_测试_配天螺杆定位 == testMode)
|
||||
screwTest();
|
||||
else if(keSG_测试_配天定位盘定位 == testMode)
|
||||
locatingPlateTest();
|
||||
else if (keSG_测试_配天新定位盘定位 == testMode)
|
||||
newLocatingPlateTest();
|
||||
else if(keSG_测试_棒材抓取 == testMode)
|
||||
rodPositionTest();
|
||||
else if(keSG_测试_筑裕钢筋焊缝定位 == testMode)
|
||||
rodWeldSeamPosition_test();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
@ -14,6 +14,8 @@ SG_APISHARED_EXPORT double vec3_dotMultiply(const SVzNL3DPoint& a, const SVzNL3D
|
||||
SG_APISHARED_EXPORT double vec3_length(const SVzNL3DPoint& a);
|
||||
// 归一化（单位向量）
|
||||
SG_APISHARED_EXPORT SVzNL3DPoint vec3_normalize(const SVzNL3DPoint& a);
|
||||
// 计算两个向量夹角（返回 角度）
|
||||
SG_APISHARED_EXPORT float vec3_computeVecAngle(const SVzNL3DPoint& a, const SVzNL3DPoint& b);
|
||||
|
||||
/**
|
||||
* @brief 平面内向量 v 绕平面法向量 n 旋转 theta 弧度
|
||||
@ -246,6 +248,26 @@ SG_APISHARED_EXPORT void wd_searchCornerPeaks(
|
||||
std::vector< SSG_pntDirAngle>& cornerPeakM
|
||||
);
|
||||
|
||||
//提取backwardAngle极值（较早实现函数可以使用此函数进行代码优化）
|
||||
SG_APISHARED_EXPORT void wd_searchBackwardAnglePeaks(
|
||||
std::vector< SSG_pntDirAngle>& corners,
|
||||
std::vector< SVzNL3DPosition>& vldPts,
|
||||
const double minCornerTh,
|
||||
double cornerMergeScale,
|
||||
std::vector< SSG_pntDirAngle>& backwardAnglePeakP,
|
||||
std::vector< SSG_pntDirAngle>& backwardAnglePeakM
|
||||
);
|
||||
|
||||
//提取forwardAngle极值（较早实现函数可以使用此函数进行代码优化）
|
||||
SG_APISHARED_EXPORT void wd_searchForwardAnglePeaks(
|
||||
std::vector< SSG_pntDirAngle>& corners,
|
||||
std::vector< SVzNL3DPosition>& vldPts,
|
||||
const double minCornerTh,
|
||||
double cornerMergeScale,
|
||||
std::vector< SSG_pntDirAngle>& forwardAnglePeakP,
|
||||
std::vector< SSG_pntDirAngle>& forwardAnglePeakM
|
||||
);
|
||||
|
||||
/// <summary>
|
||||
/// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。
|
||||
/// nPointIdx被重新定义成Feature类型
|
||||
@ -262,6 +284,17 @@ SG_APISHARED_EXPORT void wd_getLineCorerFeature(
|
||||
std::vector<SSG_basicFeature1D>& line_cornerFeatures //拐点
|
||||
);
|
||||
|
||||
/// <summary>
|
||||
/// 提取激光线上的与XY平面垂直的特征（垂直段）
|
||||
/// </summary>
|
||||
SG_APISHARED_EXPORT void wd_getXYVertialFeature_peakCornerMethod(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
int lineIdx,
|
||||
const double peakChkWin,
|
||||
const SSG_cornerParam cornerPara,
|
||||
std::vector<SSG_RUN>& xyVerticalSegs //环
|
||||
);
|
||||
|
||||
/// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。
|
||||
/// 使用平均点距进行加速
|
||||
/// nPointIdx被重新定义成Feature类型
|
||||
@ -625,6 +658,11 @@ SG_APISHARED_EXPORT double fitCircleByLeastSquare(
|
||||
const std::vector<SVzNL3DPoint>& pointArray,
|
||||
SVzNL3DPoint& center,
|
||||
double& radius);
|
||||
//圆最小二乘拟合
|
||||
SG_APISHARED_EXPORT double fitCircleByLeastSquare_2(
|
||||
const std::vector<SVzNL3DPosition>& pointArray,
|
||||
SVzNL3DPoint& center,
|
||||
double& radius);
|
||||
|
||||
//抛物线最小二乘拟合 y=ax^2 + bx + c
|
||||
SG_APISHARED_EXPORT bool leastSquareParabolaFitEigen(
|
||||
|
||||
@ -52,6 +52,20 @@ SVzNL3DPoint vec3_normalize(const SVzNL3DPoint& a)
|
||||
return result;
|
||||
}
|
||||
|
||||
// 计算两个向量夹角（返回 角度）
|
||||
float vec3_computeVecAngle(const SVzNL3DPoint& a, const SVzNL3DPoint& b)
|
||||
{
|
||||
float l = vec3_length(a) * vec3_length(b);
|
||||
if (l < 1e-6f)
|
||||
return 0.0f; // 避免除零
|
||||
|
||||
float cosTheta = vec3_dotMultiply(a, b) / l;
|
||||
cosTheta = std::max(std::min(cosTheta, 1.0f), -1.0f); // 防止数值越界
|
||||
float rad = acosf(cosTheta);
|
||||
float degree = rad * 180.0f / (float)M_PI;
|
||||
return degree;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 平面内向量 v 绕平面法向量 n 旋转 theta 弧度
|
||||
* （v 必须在平面内，自动使用简化版罗德里格斯）
|
||||
|
||||
@ -4672,7 +4672,7 @@ void wd_searchCornerPeaks(
|
||||
if (i == 275)
|
||||
int kkk = 1;
|
||||
SSG_pntDirAngle* curr_data = &corners[i];
|
||||
if (curr_data->pntIdx < 0)
|
||||
if ( (curr_data->pntIdx < 0) || (curr_data->type < 0))
|
||||
{
|
||||
if (i == i_max - 1) //最后一个
|
||||
{
|
||||
@ -4828,6 +4828,366 @@ void wd_searchCornerPeaks(
|
||||
}
|
||||
}
|
||||
|
||||
//提取backwardAngle极值（较早实现函数可以使用此函数进行代码优化）
|
||||
void wd_searchBackwardAnglePeaks(
|
||||
std::vector< SSG_pntDirAngle>& corners,
|
||||
std::vector< SVzNL3DPosition>& vldPts,
|
||||
const double minCornerTh,
|
||||
double cornerMergeScale,
|
||||
std::vector< SSG_pntDirAngle>& backwardAnglePeakP,
|
||||
std::vector< SSG_pntDirAngle>& backwardAnglePeakM
|
||||
)
|
||||
{
|
||||
int cornerSize = (int)corners.size();
|
||||
std::vector< SSG_pntDirAngle> cornerPk_P;
|
||||
std::vector< SSG_pntDirAngle> cornerPk_M;
|
||||
//搜索拐角极值
|
||||
int _state = 0;
|
||||
int pre_i = -1;
|
||||
int sEdgePtIdx = -1;
|
||||
int eEdgePtIdx = -1;
|
||||
SSG_pntDirAngle* pre_data = NULL;
|
||||
for (int i = 0, i_max = cornerSize; i < i_max; i++)
|
||||
{
|
||||
if (i == 275)
|
||||
int kkk = 1;
|
||||
SSG_pntDirAngle* curr_data = &corners[i];
|
||||
if ((curr_data->pntIdx < 0) || (curr_data->type < 0))
|
||||
{
|
||||
if (i == i_max - 1) //最后一个
|
||||
{
|
||||
if (1 == _state) //上升
|
||||
{
|
||||
if (corners[eEdgePtIdx].backwardAngle > 0)
|
||||
cornerPk_P.push_back(corners[eEdgePtIdx]);
|
||||
}
|
||||
else if (2 == _state) //下降
|
||||
{
|
||||
if (corners[eEdgePtIdx].backwardAngle < 0)
|
||||
cornerPk_M.push_back(corners[eEdgePtIdx]);
|
||||
}
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
if (NULL == pre_data)
|
||||
{
|
||||
sEdgePtIdx = i;
|
||||
eEdgePtIdx = i;
|
||||
pre_data = curr_data;
|
||||
pre_i = i;
|
||||
continue;
|
||||
}
|
||||
|
||||
eEdgePtIdx = i;
|
||||
double backwardAngleDiff = curr_data->backwardAngle - pre_data->backwardAngle;
|
||||
switch (_state)
|
||||
{
|
||||
case 0: //初态
|
||||
if (backwardAngleDiff < 0) //下降
|
||||
{
|
||||
_state = 2;
|
||||
}
|
||||
else if (backwardAngleDiff > 0) //上升
|
||||
{
|
||||
_state = 1;
|
||||
}
|
||||
break;
|
||||
case 1: //上升
|
||||
if (backwardAngleDiff < 0) //下降
|
||||
{
|
||||
if (pre_data->backwardAngle > 0)
|
||||
cornerPk_P.push_back(*pre_data);
|
||||
_state = 2;
|
||||
}
|
||||
break;
|
||||
case 2: //下降
|
||||
if (backwardAngleDiff > 0) // 上升
|
||||
{
|
||||
if (pre_data->backwardAngle < 0)
|
||||
cornerPk_M.push_back(*pre_data);
|
||||
_state = 1;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
_state = 0;
|
||||
break;
|
||||
}
|
||||
pre_data = curr_data;
|
||||
pre_i = i;
|
||||
}
|
||||
//注意：最后一个不处理，为基座位置
|
||||
|
||||
//极小值点（峰顶）
|
||||
//极值比较，在尺度窗口下寻找局部极值点
|
||||
double square_distTh = cornerMergeScale * cornerMergeScale; //2倍的cornerScale。
|
||||
for (int i = 0, i_max = (int)cornerPk_P.size(); i < i_max; i++)
|
||||
{
|
||||
if (cornerPk_P[i].backwardAngle < minCornerTh)
|
||||
continue;
|
||||
|
||||
bool isPeak = true;
|
||||
//向前搜索
|
||||
int cornerPtIdx = cornerPk_P[i].pntIdx;
|
||||
for (int j = i - 1; j >= 0; j--)
|
||||
{
|
||||
int prePtIdx = cornerPk_P[j].pntIdx;
|
||||
double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ;
|
||||
if (dist > square_distTh) //超出尺度窗口
|
||||
break;
|
||||
|
||||
if (cornerPk_P[i].backwardAngle < cornerPk_P[j].backwardAngle)
|
||||
{
|
||||
isPeak = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//向后搜索
|
||||
if (true == isPeak)
|
||||
{
|
||||
cornerPtIdx = cornerPk_P[i].pntIdx;
|
||||
for (int j = i + 1; j < i_max; j++)
|
||||
{
|
||||
int postPtIdx = cornerPk_P[j].pntIdx;
|
||||
double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2);
|
||||
if (dist > square_distTh) //超出尺度窗口
|
||||
break;
|
||||
|
||||
if (cornerPk_P[i].backwardAngle < cornerPk_P[j].backwardAngle)
|
||||
{
|
||||
isPeak = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (true == isPeak)
|
||||
backwardAnglePeakP.push_back(cornerPk_P[i]);
|
||||
}
|
||||
|
||||
for (int i = 0, i_max = (int)cornerPk_M.size(); i < i_max; i++)
|
||||
{
|
||||
if (abs(cornerPk_M[i].backwardAngle) < minCornerTh)
|
||||
continue;
|
||||
|
||||
bool isPeak = true;
|
||||
//向前搜索
|
||||
int cornerPtIdx = cornerPk_M[i].pntIdx;
|
||||
for (int j = i - 1; j >= 0; j--)
|
||||
{
|
||||
int prePtIdx = cornerPk_M[j].pntIdx;
|
||||
double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ;
|
||||
if (dist > square_distTh) //超出尺度窗口
|
||||
break;
|
||||
|
||||
if (abs(cornerPk_M[i].backwardAngle) < abs(cornerPk_M[j].backwardAngle))
|
||||
{
|
||||
isPeak = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//向后搜索
|
||||
if (true == isPeak)
|
||||
{
|
||||
cornerPtIdx = cornerPk_M[i].pntIdx;
|
||||
for (int j = i + 1; j < i_max; j++)
|
||||
{
|
||||
int postPtIdx = cornerPk_M[j].pntIdx;
|
||||
double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2);
|
||||
if (dist > square_distTh) //超出尺度窗口
|
||||
break;
|
||||
|
||||
if (abs(cornerPk_M[i].backwardAngle) < abs(cornerPk_M[j].backwardAngle))
|
||||
{
|
||||
isPeak = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (true == isPeak)
|
||||
backwardAnglePeakM.push_back(cornerPk_M[i]);
|
||||
}
|
||||
}
|
||||
|
||||
//提取forwardAngle极值（较早实现函数可以使用此函数进行代码优化）
|
||||
void wd_searchForwardAnglePeaks(
|
||||
std::vector< SSG_pntDirAngle>& corners,
|
||||
std::vector< SVzNL3DPosition>& vldPts,
|
||||
const double minCornerTh,
|
||||
double cornerMergeScale,
|
||||
std::vector< SSG_pntDirAngle>& forwardAnglePeakP,
|
||||
std::vector< SSG_pntDirAngle>& forwardAnglePeakM
|
||||
)
|
||||
{
|
||||
int cornerSize = (int)corners.size();
|
||||
std::vector< SSG_pntDirAngle> cornerPk_P;
|
||||
std::vector< SSG_pntDirAngle> cornerPk_M;
|
||||
//搜索拐角极值
|
||||
int _state = 0;
|
||||
int pre_i = -1;
|
||||
int sEdgePtIdx = -1;
|
||||
int eEdgePtIdx = -1;
|
||||
SSG_pntDirAngle* pre_data = NULL;
|
||||
for (int i = 0, i_max = cornerSize; i < i_max; i++)
|
||||
{
|
||||
if (i == 275)
|
||||
int kkk = 1;
|
||||
SSG_pntDirAngle* curr_data = &corners[i];
|
||||
if ((curr_data->pntIdx < 0) || (curr_data->type < 0))
|
||||
{
|
||||
if (i == i_max - 1) //最后一个
|
||||
{
|
||||
if (1 == _state) //上升
|
||||
{
|
||||
if (corners[eEdgePtIdx].forwardAngle > 0)
|
||||
cornerPk_P.push_back(corners[eEdgePtIdx]);
|
||||
}
|
||||
else if (2 == _state) //下降
|
||||
{
|
||||
if (corners[eEdgePtIdx].forwardAngle < 0)
|
||||
cornerPk_M.push_back(corners[eEdgePtIdx]);
|
||||
}
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
if (NULL == pre_data)
|
||||
{
|
||||
sEdgePtIdx = i;
|
||||
eEdgePtIdx = i;
|
||||
pre_data = curr_data;
|
||||
pre_i = i;
|
||||
continue;
|
||||
}
|
||||
|
||||
eEdgePtIdx = i;
|
||||
double forwardAngleDiff = curr_data->forwardAngle - pre_data->forwardAngle;
|
||||
switch (_state)
|
||||
{
|
||||
case 0: //初态
|
||||
if (forwardAngleDiff < 0) //下降
|
||||
{
|
||||
_state = 2;
|
||||
}
|
||||
else if (forwardAngleDiff > 0) //上升
|
||||
{
|
||||
_state = 1;
|
||||
}
|
||||
break;
|
||||
case 1: //上升
|
||||
if (forwardAngleDiff < 0) //下降
|
||||
{
|
||||
if (pre_data->forwardAngle > 0)
|
||||
cornerPk_P.push_back(*pre_data);
|
||||
_state = 2;
|
||||
}
|
||||
break;
|
||||
case 2: //下降
|
||||
if (forwardAngleDiff > 0) // 上升
|
||||
{
|
||||
if (pre_data->forwardAngle < 0)
|
||||
cornerPk_M.push_back(*pre_data);
|
||||
_state = 1;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
_state = 0;
|
||||
break;
|
||||
}
|
||||
pre_data = curr_data;
|
||||
pre_i = i;
|
||||
}
|
||||
//注意：最后一个不处理，为基座位置
|
||||
|
||||
//极小值点（峰顶）
|
||||
//极值比较，在尺度窗口下寻找局部极值点
|
||||
double square_distTh = cornerMergeScale * cornerMergeScale; //2倍的cornerScale。
|
||||
for (int i = 0, i_max = (int)cornerPk_P.size(); i < i_max; i++)
|
||||
{
|
||||
if (cornerPk_P[i].forwardAngle < minCornerTh)
|
||||
continue;
|
||||
|
||||
bool isPeak = true;
|
||||
//向前搜索
|
||||
int cornerPtIdx = cornerPk_P[i].pntIdx;
|
||||
for (int j = i - 1; j >= 0; j--)
|
||||
{
|
||||
int prePtIdx = cornerPk_P[j].pntIdx;
|
||||
double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ;
|
||||
if (dist > square_distTh) //超出尺度窗口
|
||||
break;
|
||||
|
||||
if (cornerPk_P[i].forwardAngle < cornerPk_P[j].forwardAngle)
|
||||
{
|
||||
isPeak = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//向后搜索
|
||||
if (true == isPeak)
|
||||
{
|
||||
cornerPtIdx = cornerPk_P[i].pntIdx;
|
||||
for (int j = i + 1; j < i_max; j++)
|
||||
{
|
||||
int postPtIdx = cornerPk_P[j].pntIdx;
|
||||
double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2);
|
||||
if (dist > square_distTh) //超出尺度窗口
|
||||
break;
|
||||
|
||||
if (cornerPk_P[i].forwardAngle < cornerPk_P[j].forwardAngle)
|
||||
{
|
||||
isPeak = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (true == isPeak)
|
||||
forwardAnglePeakP.push_back(cornerPk_P[i]);
|
||||
}
|
||||
|
||||
for (int i = 0, i_max = (int)cornerPk_M.size(); i < i_max; i++)
|
||||
{
|
||||
if (abs(cornerPk_M[i].forwardAngle) < minCornerTh)
|
||||
continue;
|
||||
|
||||
bool isPeak = true;
|
||||
//向前搜索
|
||||
int cornerPtIdx = cornerPk_M[i].pntIdx;
|
||||
for (int j = i - 1; j >= 0; j--)
|
||||
{
|
||||
int prePtIdx = cornerPk_M[j].pntIdx;
|
||||
double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ;
|
||||
if (dist > square_distTh) //超出尺度窗口
|
||||
break;
|
||||
|
||||
if (abs(cornerPk_M[i].forwardAngle) < abs(cornerPk_M[j].forwardAngle))
|
||||
{
|
||||
isPeak = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//向后搜索
|
||||
if (true == isPeak)
|
||||
{
|
||||
cornerPtIdx = cornerPk_M[i].pntIdx;
|
||||
for (int j = i + 1; j < i_max; j++)
|
||||
{
|
||||
int postPtIdx = cornerPk_M[j].pntIdx;
|
||||
double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2);
|
||||
if (dist > square_distTh) //超出尺度窗口
|
||||
break;
|
||||
|
||||
if (abs(cornerPk_M[i].forwardAngle) < abs(cornerPk_M[j].forwardAngle))
|
||||
{
|
||||
isPeak = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (true == isPeak)
|
||||
forwardAnglePeakM.push_back(cornerPk_M[i]);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。
|
||||
/// nPointIdx被重新定义成Feature类型
|
||||
@ -5019,6 +5379,124 @@ void wd_getLineCorerFeature(
|
||||
return;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 提取激光线上的与XY平面垂直的特征（垂直段）
|
||||
/// seg端点：z距离大于门限
|
||||
/// nPointIdx被重新定义成Feature类型
|
||||
/// 算法流程：
|
||||
/// （1）逐点计算前向角和后向角
|
||||
/// （2）搜索同方向的拐角连续段
|
||||
/// （2）搜索Z极值，
|
||||
/// （2）逐点计算拐角，顺时针为负，逆时针为正
|
||||
/// （3）搜索正拐角的极大值。
|
||||
/// （4）判断拐角是否为跳变
|
||||
///
|
||||
/// </summary>
|
||||
void wd_getXYVertialFeature_peakCornerMethod(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
int lineIdx,
|
||||
const double peakChkWin,
|
||||
const SSG_cornerParam cornerPara,
|
||||
std::vector<SSG_RUN>& xyVerticalSegs
|
||||
)
|
||||
{
|
||||
double arcPerPointCornerMinValue = 5; //arc上每个点的转角最小值
|
||||
double arcTotalCornerMinValue = 30; //整个Arc的转角最小值
|
||||
//计算前向角和后向角
|
||||
std::vector< SSG_pntDirAngle> ptDirAngles;
|
||||
wd_computeDirAngle_wholeLine(lineData, cornerPara, ptDirAngles);
|
||||
|
||||
double cornerMergeScale = cornerPara.scale;
|
||||
//提取backwardAngle极值（较早实现函数可以使用此函数进行代码优化）
|
||||
std::vector< SSG_pntDirAngle> backwardAnglePeakP;
|
||||
std::vector< SSG_pntDirAngle> backwardAnglePeakM;
|
||||
wd_searchBackwardAnglePeaks(
|
||||
ptDirAngles,
|
||||
lineData,
|
||||
cornerPara.cornerTh,
|
||||
cornerMergeScale,
|
||||
backwardAnglePeakP,
|
||||
backwardAnglePeakM
|
||||
);
|
||||
|
||||
//提取forwardAngle极值（较早实现函数可以使用此函数进行代码优化）
|
||||
std::vector< SSG_pntDirAngle> forwardAnglePeakP;
|
||||
std::vector< SSG_pntDirAngle> forwardAnglePeakM;
|
||||
wd_searchForwardAnglePeaks(
|
||||
ptDirAngles,
|
||||
lineData,
|
||||
cornerPara.cornerTh,
|
||||
cornerMergeScale,
|
||||
forwardAnglePeakP,
|
||||
forwardAnglePeakM
|
||||
);
|
||||
|
||||
//匹配forwardAnglePeakM和backwardAnglePeakM, forwardAnglePeakM在前，backwardAnglePeakM在后
|
||||
for (int i = 0; i < (int)forwardAnglePeakM.size(); i++)
|
||||
{
|
||||
for (int j = 0; j < (int)backwardAnglePeakM.size(); j++)
|
||||
{
|
||||
if (backwardAnglePeakM[j].pntIdx > forwardAnglePeakM[i].pntIdx)
|
||||
{
|
||||
int pntIdx_1 = forwardAnglePeakM[i].pntIdx;
|
||||
int pntIdx_2 = backwardAnglePeakM[j].pntIdx;
|
||||
//计算两者的角度
|
||||
double diffY = abs(lineData[pntIdx_2].pt3D.y - lineData[pntIdx_1].pt3D.y);
|
||||
if (diffY < cornerMergeScale)
|
||||
{
|
||||
double tanValue = (lineData[pntIdx_2].pt3D.z - lineData[pntIdx_1].pt3D.z) / diffY;
|
||||
double angle = atan(tanValue) * 180.0 / PI;
|
||||
if (abs(angle) > cornerPara.cornerTh)
|
||||
{
|
||||
SSG_RUN a_seg;
|
||||
if(lineData[pntIdx_1].pt3D.z < lineData[pntIdx_2].pt3D.z)
|
||||
a_seg.value = 1;
|
||||
else
|
||||
a_seg.value = -1;
|
||||
a_seg.start = pntIdx_1;
|
||||
a_seg.len = pntIdx_2 - pntIdx_1 +1;
|
||||
xyVerticalSegs.push_back(a_seg);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//匹配forwardAnglePeakP和backwardAnglePeakP, forwardAnglePeakP在前，backwardAnglePeakP在后
|
||||
for (int i = 0; i < (int)forwardAnglePeakP.size(); i++)
|
||||
{
|
||||
for (int j = 0; j < (int)backwardAnglePeakP.size(); j++)
|
||||
{
|
||||
if (backwardAnglePeakP[j].pntIdx > forwardAnglePeakP[i].pntIdx)
|
||||
{
|
||||
int pntIdx_1 = forwardAnglePeakP[i].pntIdx;
|
||||
int pntIdx_2 = backwardAnglePeakP[j].pntIdx;
|
||||
//计算两者的角度
|
||||
double diffY = abs(lineData[pntIdx_2].pt3D.y - lineData[pntIdx_1].pt3D.y);
|
||||
if (diffY < cornerMergeScale)
|
||||
{
|
||||
double tanValue = (lineData[pntIdx_2].pt3D.z - lineData[pntIdx_1].pt3D.z) / diffY;
|
||||
double angle = atan(tanValue) * 180.0 / PI;
|
||||
if (abs(angle) > cornerPara.cornerTh)
|
||||
{
|
||||
SSG_RUN a_seg;
|
||||
if (lineData[pntIdx_1].pt3D.z < lineData[pntIdx_2].pt3D.z)
|
||||
a_seg.value = 1;
|
||||
else
|
||||
a_seg.value = -1;
|
||||
a_seg.start = pntIdx_1;
|
||||
a_seg.len = pntIdx_2 - pntIdx_1 + 1;
|
||||
xyVerticalSegs.push_back(a_seg);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。
|
||||
/// 使用平均点距进行加速
|
||||
/// nPointIdx被重新定义成Feature类型
|
||||
|
||||
@ -187,6 +187,72 @@ double fitCircleByLeastSquare(
|
||||
return err;
|
||||
}
|
||||
|
||||
//Ô²×îС¶þ³ËÄâºÏ
|
||||
double fitCircleByLeastSquare_2(
|
||||
const std::vector<SVzNL3DPosition>& 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].pt3D.x;
|
||||
sumY += pointArray[pId].pt3D.y;
|
||||
|
||||
double x2 = pointArray[pId].pt3D.x * pointArray[pId].pt3D.x;
|
||||
double y2 = pointArray[pId].pt3D.y * pointArray[pId].pt3D.y;
|
||||
sumX2 += x2;
|
||||
sumY2 += y2;
|
||||
|
||||
sumX3 += x2 * pointArray[pId].pt3D.x;
|
||||
sumY3 += y2 * pointArray[pId].pt3D.y;
|
||||
sumXY += pointArray[pId].pt3D.x * pointArray[pId].pt3D.y;
|
||||
sumXY2 += pointArray[pId].pt3D.x * y2;
|
||||
sumX2Y += x2 * pointArray[pId].pt3D.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].pt3D.x - center.x, 2) + pow(pointArray[pId].pt3D.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,
|
||||
|
||||
@ -20,7 +20,8 @@
|
||||
//version 1.2.9 : 修正螺杆定位算法中轴向拟合计算时方向确定的问题，保证投影后的点云与投影前的点云的Z变化趋势一致
|
||||
//version 1.2.10 : 改进螺杆定位取点算法
|
||||
//version 1.2.11 : 根据现场数据调整特征生长参数
|
||||
std::string m_strVersion = "1.2.11";
|
||||
//version 1.3.0 : 新的定位盘中心测量功能
|
||||
std::string m_strVersion = "1.3.0";
|
||||
const char* wd_rodAndBarDetectionVersion(void)
|
||||
{
|
||||
return m_strVersion.c_str();
|
||||
@ -1921,6 +1922,516 @@ SSX_platePoseInfo sx_getLocationPlatePose(
|
||||
return resultPose;
|
||||
}
|
||||
|
||||
//新的计算定位盘中心点位姿（去除了定位盘，只有定位板）
|
||||
SSX_platePoseInfo sx_getLocationPlatePose_new(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SSG_cornerParam cornerPara,
|
||||
int* errCode)
|
||||
{
|
||||
*errCode = 0;
|
||||
SSX_platePoseInfo resultPose;
|
||||
resultPose.center = { 0, 0, 0 };
|
||||
resultPose.normalDir = { 0, 0, 0 };
|
||||
resultPose.holeLT = { 0, 0, 0 };
|
||||
resultPose.holeRB = { 0, 0, 0 };
|
||||
resultPose.xDir = { 0, 0, 0 };
|
||||
resultPose.yDir = { 0, 0, 0 };
|
||||
int lineNum = (int)scanLines.size();
|
||||
if (lineNum == 0)
|
||||
{
|
||||
*errCode = SG_ERR_3D_DATA_NULL;
|
||||
return resultPose;
|
||||
}
|
||||
|
||||
int linePtNum = (int)scanLines[0].size();
|
||||
|
||||
//判断数据格式是否为grid。算法只能处理grid数据格式
|
||||
bool isGridData = true;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
if (linePtNum != (int)scanLines[line].size())
|
||||
{
|
||||
isGridData = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (false == isGridData)//数据不是网格格式
|
||||
{
|
||||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||||
return resultPose;
|
||||
}
|
||||
|
||||
//产生水平扫描数据
|
||||
std::vector< std::vector<SVzNL3DPosition>> scanLines_h;
|
||||
scanLines_h.resize(linePtNum);
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
scanLines_h[i].resize(lineNum);
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
for (int j = 0; j < linePtNum; j++)
|
||||
{
|
||||
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||||
scanLines_h[j][line] = scanLines[line][j];
|
||||
scanLines_h[j][line].pt3D.x = scanLines[line][j].pt3D.y;
|
||||
scanLines_h[j][line].pt3D.y = scanLines[line][j].pt3D.x;
|
||||
}
|
||||
}
|
||||
for (int line = 0; line < linePtNum; line++)
|
||||
{
|
||||
for (int j = 0, j_max = (int)scanLines_h[line].size(); j < j_max; j++)
|
||||
scanLines_h[line][j].nPointIdx = j;
|
||||
}
|
||||
|
||||
//算法流程：
|
||||
//1、检查垂直方向数据并去除
|
||||
//2、聚类
|
||||
//3、保留最前面目标
|
||||
//4、提取孔
|
||||
//5、拟合
|
||||
//6、计算中间坐标
|
||||
|
||||
std::vector<std::vector<int>> flags;
|
||||
flags.resize(lineNum);
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
flags[i].resize(linePtNum);
|
||||
std::fill(flags[i].begin(), flags[i].end(), 0);
|
||||
}
|
||||
std::vector<std::vector<SSG_RUN>> zVertivalSegs;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
if (line == 700)
|
||||
int kkk = 1;
|
||||
std::vector<SSG_RUN> line_verticalSegs;
|
||||
wd_getXYVertialFeature_peakCornerMethod(
|
||||
scanLines[line],
|
||||
line,
|
||||
cornerPara.scale,
|
||||
cornerPara,
|
||||
line_verticalSegs
|
||||
);
|
||||
zVertivalSegs.push_back(line_verticalSegs);
|
||||
|
||||
for (int i = 0; i < (int)line_verticalSegs.size(); i++)
|
||||
{
|
||||
int idx_0 = line_verticalSegs[i].start;
|
||||
for (int j = 0; j < line_verticalSegs[i].len; j++)
|
||||
flags[line][idx_0 + j] = 1;
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<std::vector<SSG_RUN>> zVertivalSegs_h;
|
||||
for (int line = 0; line < linePtNum; line++)
|
||||
{
|
||||
if (line == 1177)
|
||||
int kkk = 1;
|
||||
std::vector<SSG_RUN> line_verticalSegs;
|
||||
wd_getXYVertialFeature_peakCornerMethod(
|
||||
scanLines_h[line],
|
||||
line,
|
||||
cornerPara.scale,
|
||||
cornerPara,
|
||||
line_verticalSegs
|
||||
);
|
||||
zVertivalSegs_h.push_back(line_verticalSegs);
|
||||
|
||||
for (int i = 0; i < (int)line_verticalSegs.size(); i++)
|
||||
{
|
||||
int idx_0 = line_verticalSegs[i].start;
|
||||
for (int j = 0; j < line_verticalSegs[i].len; j++)
|
||||
flags[idx_0+j][line] = 1;
|
||||
}
|
||||
}
|
||||
//标注
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
for (int j = 0; j < linePtNum; j++)
|
||||
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||||
}
|
||||
//将垂直线段去除
|
||||
std::vector< SVzNL3DPosition> validPoints;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
for (int j = 0; j < linePtNum; j++)
|
||||
{
|
||||
if (flags[line][j] > 0)
|
||||
scanLines[line][j].pt3D.z = 0;
|
||||
|
||||
if (scanLines[line][j].pt3D.z > 1e-4)
|
||||
{
|
||||
SVzNL3DPosition a_vldPt;
|
||||
a_vldPt.pt3D = scanLines[line][j].pt3D;
|
||||
a_vldPt.nPointIdx = (line << 16) | (j & 0xffff);
|
||||
validPoints.push_back(a_vldPt);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//聚类
|
||||
//内部参数
|
||||
double minObjSize_w = 150;
|
||||
double minObjSize_h = 150;
|
||||
|
||||
int clusterCheckWin = 5;
|
||||
double clusterDist = 5.0;
|
||||
int distType = 1; //0 - 2d distance; 1- 3d distance
|
||||
std::vector<std::vector< SVzNL3DPosition>> objClusters; //result
|
||||
wd_pointClustering_speedUp(
|
||||
validPoints,
|
||||
lineNum, linePtNum, clusterCheckWin, //搜索窗口
|
||||
clusterDist,
|
||||
distType,
|
||||
objClusters //result
|
||||
);
|
||||
|
||||
//保留最前面的目标
|
||||
std::vector<double> objMeanZ;
|
||||
objMeanZ.resize(objClusters.size());
|
||||
for (int i = 0; i < (int)objClusters.size(); i++)
|
||||
{
|
||||
SSG_ROIRectD a_roi = _getListROI(objClusters[i]);
|
||||
double w = a_roi.right - a_roi.left;
|
||||
double h = a_roi.bottom - a_roi.top;
|
||||
|
||||
double meanZ = _getListMeanZ(objClusters[i]);
|
||||
objMeanZ[i] = meanZ;
|
||||
}
|
||||
//选出z最小的目标作为顶部轮廓
|
||||
int objIdx = -1;
|
||||
double minMeanZ = DBL_MAX;
|
||||
for (int i = 0; i < (int)objClusters.size(); i++)
|
||||
{
|
||||
if (objMeanZ[i] > 1e-4)
|
||||
{
|
||||
if (minMeanZ > objMeanZ[i])
|
||||
{
|
||||
minMeanZ = objMeanZ[i];
|
||||
objIdx = i;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (objIdx < 0)
|
||||
{
|
||||
*errCode = SG_ERR_ZERO_OBJECTS;
|
||||
return resultPose;
|
||||
}
|
||||
|
||||
std::vector< SVzNL3DPosition>& topCluster = objClusters[objIdx];
|
||||
//标注
|
||||
//重新将flags设置为目标的mask
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
std::fill(flags[i].begin(), flags[i].end(), -1);
|
||||
for (int i = 0; i < (int)topCluster.size(); i++)
|
||||
{
|
||||
int line = topCluster[i].nPointIdx >> 16;
|
||||
int ptIdx = topCluster[i].nPointIdx & 0x0000FFFF;
|
||||
scanLines[line][ptIdx].nPointIdx = 2;
|
||||
flags[line][ptIdx] = i; //indexing
|
||||
}
|
||||
|
||||
|
||||
//拟合平面
|
||||
std::vector<cv::Point3f> Points3ds;
|
||||
for (int i = 0; i < (int)topCluster.size(); i++)
|
||||
{
|
||||
cv::Point3f a_pt = cv::Point3f(topCluster[i].pt3D.x, topCluster[i].pt3D.y, topCluster[i].pt3D.z);
|
||||
Points3ds.push_back(a_pt);
|
||||
}
|
||||
//计算面参数: z = Ax + By + C
|
||||
//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
|
||||
#if 1
|
||||
std::vector<cv::Point3f> out_inliers;
|
||||
Plane res = ransacFitPlane(Points3ds, out_inliers);
|
||||
if (res.C < 0)
|
||||
{
|
||||
res.A = -res.A;
|
||||
res.B = -res.B;
|
||||
res.C = -res.C;
|
||||
res.D = -res.D;
|
||||
}
|
||||
#else
|
||||
Plane res = robustFitPlane(Points3ds);
|
||||
#endif
|
||||
|
||||
//计算投影向量
|
||||
SVzNL3DPoint vec_1 = { res.A, res.B, res.C };
|
||||
SVzNL3DPoint vec_2 = { 0, 0, 1.0 };
|
||||
SSG_planeCalibPara poseR = wd_computeRTMatrix(vec_1, vec_2);
|
||||
|
||||
//投影
|
||||
double normDataPlane = sqrt(res.A * res.A + res.B * res.B + res.C * res.C);
|
||||
std::vector<SVzNL3DPosition> projectPoints3ds;
|
||||
projectPoints3ds.resize(topCluster.size());
|
||||
double sum_x = 0, sum_y = 0, sum_z = 0;
|
||||
int sumZConter = 0;
|
||||
for (int i = 0; i < (int)topCluster.size(); i++)
|
||||
{
|
||||
double distToPlane = abs(res.A * topCluster[i].pt3D.x + res.B * topCluster[i].pt3D.y + res.C * topCluster[i].pt3D.z + res.D) / normDataPlane;
|
||||
double x = topCluster[i].pt3D.x * poseR.planeCalib[0] + topCluster[i].pt3D.y * poseR.planeCalib[1] + topCluster[i].pt3D.z * poseR.planeCalib[2];
|
||||
double y = topCluster[i].pt3D.x * poseR.planeCalib[3] + topCluster[i].pt3D.y * poseR.planeCalib[4] + topCluster[i].pt3D.z * poseR.planeCalib[5];
|
||||
double z = topCluster[i].pt3D.x * poseR.planeCalib[6] + topCluster[i].pt3D.y * poseR.planeCalib[7] + topCluster[i].pt3D.z * poseR.planeCalib[8];
|
||||
projectPoints3ds[i].nPointIdx = topCluster[i].nPointIdx;
|
||||
projectPoints3ds[i].pt3D = { x, y, z };
|
||||
sum_x += x;
|
||||
sum_y += y;
|
||||
if (distToPlane < 2.0)
|
||||
{
|
||||
sum_z += z;
|
||||
sumZConter++;
|
||||
}
|
||||
}
|
||||
if(sumZConter == 0)
|
||||
{
|
||||
*errCode = SG_ERR_ZERO_OBJECTS;
|
||||
return resultPose;
|
||||
}
|
||||
double meanZ = sum_z / sumZConter;
|
||||
|
||||
//生成孔的边界点
|
||||
std::vector<std::vector<int>> endingMask;
|
||||
endingMask.resize(lineNum);
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
endingMask[i].resize(linePtNum);
|
||||
std::fill(endingMask[i].begin(), endingMask[i].end(), 0);
|
||||
}
|
||||
std::vector< SVzNL3DPosition> endingPts;
|
||||
//垂直
|
||||
double topZTh = meanZ + 10.0;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
int preValid = -1;
|
||||
for (int j = 0; j < linePtNum; j++)
|
||||
{
|
||||
|
||||
if (flags[line][j] >= 0)
|
||||
{
|
||||
if (preValid == 0)
|
||||
{
|
||||
if (endingMask[line][j] == 0)
|
||||
{
|
||||
int idx = flags[line][j];
|
||||
if (projectPoints3ds[idx].pt3D.z < topZTh)
|
||||
{
|
||||
endingPts.push_back(projectPoints3ds[idx]);
|
||||
endingMask[line][j] = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
preValid = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (preValid == 1)
|
||||
{
|
||||
if (endingMask[line][j - 1] == 0)
|
||||
{
|
||||
int idx = flags[line][j - 1];
|
||||
if (projectPoints3ds[idx].pt3D.z < topZTh)
|
||||
{
|
||||
endingPts.push_back(projectPoints3ds[idx]);
|
||||
endingMask[line][j - 1] = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
preValid = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
//水平
|
||||
for (int j = 0; j < linePtNum; j++)
|
||||
{
|
||||
int preValid = -1;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
if (flags[line][j] >= 0)
|
||||
{
|
||||
if (preValid == 0)
|
||||
{
|
||||
if (endingMask[line][j] == 0)
|
||||
{
|
||||
int idx = flags[line][j];
|
||||
if (projectPoints3ds[idx].pt3D.z < topZTh)
|
||||
{
|
||||
endingPts.push_back(projectPoints3ds[idx]);
|
||||
endingMask[line][j] = 1;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
preValid = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (preValid == 1)
|
||||
{
|
||||
if (endingMask[line-1][j] == 0)
|
||||
{
|
||||
int idx = flags[line-1][j];
|
||||
if (projectPoints3ds[idx].pt3D.z < topZTh)
|
||||
{
|
||||
endingPts.push_back(projectPoints3ds[idx]);
|
||||
endingMask[line - 1][j] = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
preValid = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
//标注
|
||||
for (int i = 0; i < (int)endingPts.size(); i++)
|
||||
{
|
||||
int line = endingPts[i].nPointIdx >> 16;
|
||||
int ptIdx = endingPts[i].nPointIdx & 0x0000FFFF;
|
||||
scanLines[line][ptIdx].nPointIdx = 3;
|
||||
}
|
||||
|
||||
//孔聚类
|
||||
clusterCheckWin = 10;
|
||||
clusterDist = 5.0;
|
||||
distType = 0; //0 - 2d distance; 1- 3d distance
|
||||
std::vector<std::vector< SVzNL3DPosition>> holeClusters; //result
|
||||
wd_pointClustering_speedUp(
|
||||
endingPts,
|
||||
lineNum, linePtNum, clusterCheckWin, //搜索窗口
|
||||
clusterDist,
|
||||
distType,
|
||||
holeClusters //result
|
||||
);
|
||||
|
||||
//拟合
|
||||
std::vector< SVzNL3DPoint> holesCenter;
|
||||
holesCenter.resize(holeClusters.size());
|
||||
std::vector<double> holesR;
|
||||
holesR.resize(holeClusters.size());
|
||||
//目标圆拟合
|
||||
//圆拟合
|
||||
for (int i = 0; i < (int)holeClusters.size(); i++)
|
||||
{
|
||||
SVzNL3DPoint a_center;
|
||||
double a_radius;
|
||||
double err = fitCircleByLeastSquare_2(holeClusters[i], a_center, a_radius);
|
||||
holesCenter[i] = a_center;
|
||||
holesR[i] = a_radius;
|
||||
}
|
||||
//计算中心孔的姿态
|
||||
//目标过滤
|
||||
//首先挑出最大的孔
|
||||
const double bigHoleD = 40.0;//大孔直径
|
||||
const double smallHoleD = 5.0;//大孔直径
|
||||
std::vector<int> bigHoleIdx;
|
||||
std::vector<int> smallHoleIdx;
|
||||
for (int i = 0; i < (int)holeClusters.size(); i++)
|
||||
{
|
||||
if ( (holesR[i] > bigHoleD/2) && (holesR[i] < bigHoleD * 1.5))
|
||||
bigHoleIdx.push_back(i);
|
||||
else if ( (holesR[i] > smallHoleD/2) && (holesR[i] < smallHoleD * 1.5))
|
||||
smallHoleIdx.push_back(i);
|
||||
}
|
||||
if (bigHoleIdx.size() != 4)
|
||||
{
|
||||
*errCode = SG_ERR_ZERO_OBJECTS;
|
||||
return resultPose;
|
||||
}
|
||||
int holeL_idx = -1, holeR_idx = -1, holeT_idx = -1, holeB_idx = -1;
|
||||
for (int i = 0; i < (int)bigHoleIdx.size(); i++)
|
||||
{
|
||||
int idx = bigHoleIdx[i];
|
||||
if (holeL_idx < 0)
|
||||
holeL_idx = idx;
|
||||
else if (holesCenter[holeL_idx].x > holesCenter[idx].x)
|
||||
holeL_idx = idx;
|
||||
|
||||
if (holeR_idx < 0)
|
||||
holeR_idx = idx;
|
||||
else if (holesCenter[holeR_idx].x < holesCenter[idx].x)
|
||||
holeR_idx = idx;
|
||||
|
||||
if (holeT_idx < 0)
|
||||
holeT_idx = idx;
|
||||
else if (holesCenter[holeT_idx].y > holesCenter[idx].y)
|
||||
holeT_idx = idx;
|
||||
|
||||
if (holeB_idx < 0)
|
||||
holeB_idx = idx;
|
||||
else if (holesCenter[holeB_idx].y < holesCenter[idx].y)
|
||||
holeB_idx = idx;
|
||||
}
|
||||
double ref_x = (holesCenter[holeL_idx].x + holesCenter[holeR_idx].x + holesCenter[holeT_idx].x + holesCenter[holeB_idx].x) / 4.0;
|
||||
double ref_y = (holesCenter[holeL_idx].y + holesCenter[holeR_idx].y + holesCenter[holeT_idx].y + holesCenter[holeB_idx].y) / 4.0;
|
||||
//寻找中间孔
|
||||
int centerHoleIdx = -1;
|
||||
double minDist = -1;
|
||||
for (int i = 0; i < (int)smallHoleIdx.size(); i++)
|
||||
{
|
||||
int idx = smallHoleIdx[i];
|
||||
double dist = sqrt(pow(holesCenter[idx].x - ref_x, 2) + pow(holesCenter[idx].y - ref_y, 2));
|
||||
if (centerHoleIdx < 0)
|
||||
{
|
||||
centerHoleIdx = idx;
|
||||
minDist = dist;
|
||||
}
|
||||
else if (minDist > dist)
|
||||
{
|
||||
centerHoleIdx = idx;
|
||||
minDist = dist;
|
||||
}
|
||||
}
|
||||
|
||||
resultPose.center = { holesCenter[centerHoleIdx].x, holesCenter[centerHoleIdx].y, meanZ};
|
||||
resultPose.xDir = {0.0, 0.0, -1.0}; // { 1.0, 0.0, 0 };// { 0.0, 0.0, 1.0 };
|
||||
resultPose.normalDir = { holesCenter[holeR_idx].x - holesCenter[centerHoleIdx].x, holesCenter[holeR_idx].y - holesCenter[centerHoleIdx].y, 0.0 }; // { 1.0, 0.0, 0 };
|
||||
resultPose.normalDir = vec3_normalize(resultPose.normalDir);
|
||||
//resultPose.yDir = { 0.0, 1.0, 0 };
|
||||
|
||||
//叉乘出y;
|
||||
//向量叉乘
|
||||
resultPose.yDir = vec3_cross(resultPose.normalDir, resultPose.xDir);
|
||||
resultPose.holeLT = resultPose.center;
|
||||
resultPose.holeRB = resultPose.center;
|
||||
#if 0
|
||||
//旋转回去
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
//行处理
|
||||
//调平，去除地面
|
||||
lineDataRT_vector(scanLines[i], poseR.invRMatrix, -1);
|
||||
}
|
||||
#endif
|
||||
double x = resultPose.center.x * poseR.invRMatrix[0] + resultPose.center.y * poseR.invRMatrix[1] + resultPose.center.z * poseR.invRMatrix[2];
|
||||
double y = resultPose.center.x * poseR.invRMatrix[3] + resultPose.center.y * poseR.invRMatrix[4] + resultPose.center.z * poseR.invRMatrix[5];
|
||||
double z = resultPose.center.x * poseR.invRMatrix[6] + resultPose.center.y * poseR.invRMatrix[7] + resultPose.center.z * poseR.invRMatrix[8];
|
||||
resultPose.center = { x, y, z };
|
||||
x = resultPose.normalDir.x * poseR.invRMatrix[0] + resultPose.normalDir.y * poseR.invRMatrix[1] + resultPose.normalDir.z * poseR.invRMatrix[2];
|
||||
y = resultPose.normalDir.x * poseR.invRMatrix[3] + resultPose.normalDir.y * poseR.invRMatrix[4] + resultPose.normalDir.z * poseR.invRMatrix[5];
|
||||
z = resultPose.normalDir.x * poseR.invRMatrix[6] + resultPose.normalDir.y * poseR.invRMatrix[7] + resultPose.normalDir.z * poseR.invRMatrix[8];
|
||||
resultPose.normalDir = { x, y, z };
|
||||
|
||||
x = resultPose.xDir.x * poseR.invRMatrix[0] + resultPose.xDir.y * poseR.invRMatrix[1] + resultPose.xDir.z * poseR.invRMatrix[2];
|
||||
y = resultPose.xDir.x * poseR.invRMatrix[3] + resultPose.xDir.y * poseR.invRMatrix[4] + resultPose.xDir.z * poseR.invRMatrix[5];
|
||||
z = resultPose.xDir.x * poseR.invRMatrix[6] + resultPose.xDir.y * poseR.invRMatrix[7] + resultPose.xDir.z * poseR.invRMatrix[8];
|
||||
resultPose.xDir = { x, y, z };
|
||||
|
||||
x = resultPose.yDir.x * poseR.invRMatrix[0] + resultPose.yDir.y * poseR.invRMatrix[1] + resultPose.yDir.z * poseR.invRMatrix[2];
|
||||
y = resultPose.yDir.x * poseR.invRMatrix[3] + resultPose.yDir.y * poseR.invRMatrix[4] + resultPose.yDir.z * poseR.invRMatrix[5];
|
||||
z = resultPose.yDir.x * poseR.invRMatrix[6] + resultPose.yDir.y * poseR.invRMatrix[7] + resultPose.yDir.z * poseR.invRMatrix[8];
|
||||
resultPose.yDir = { x, y, z };
|
||||
|
||||
x = resultPose.holeLT.x * poseR.invRMatrix[0] + resultPose.holeLT.y * poseR.invRMatrix[1] + resultPose.holeLT.z * poseR.invRMatrix[2];
|
||||
y = resultPose.holeLT.x * poseR.invRMatrix[3] + resultPose.holeLT.y * poseR.invRMatrix[4] + resultPose.holeLT.z * poseR.invRMatrix[5];
|
||||
z = resultPose.holeLT.x * poseR.invRMatrix[6] + resultPose.holeLT.y * poseR.invRMatrix[7] + resultPose.holeLT.z * poseR.invRMatrix[8];
|
||||
resultPose.holeLT = { x, y, z };
|
||||
|
||||
x = resultPose.holeRB.x * poseR.invRMatrix[0] + resultPose.holeRB.y * poseR.invRMatrix[1] + resultPose.holeRB.z * poseR.invRMatrix[2];
|
||||
y = resultPose.holeRB.x * poseR.invRMatrix[3] + resultPose.holeRB.y * poseR.invRMatrix[4] + resultPose.holeRB.z * poseR.invRMatrix[5];
|
||||
z = resultPose.holeRB.x * poseR.invRMatrix[6] + resultPose.holeRB.y * poseR.invRMatrix[7] + resultPose.holeRB.z * poseR.invRMatrix[8];
|
||||
resultPose.holeRB = { x, y, z };
|
||||
|
||||
|
||||
return resultPose;
|
||||
}
|
||||
|
||||
#if 0
|
||||
void rodAarcFeatueDetection(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
@ -2208,7 +2719,8 @@ void sx_rodPositioning(
|
||||
pow(startCenter.y - endCenter.y, 2) +
|
||||
pow(startCenter.z - endCenter.z, 2));
|
||||
double lenDiff = abs(len - rodParam.len);
|
||||
if (lenDiff < rodParam.len* 0.15) //validObj
|
||||
double lenDiff2 = abs(len - 2 * rodParam.len);
|
||||
if ( (lenDiff < rodParam.len* 0.15) || (lenDiff2 < rodParam.len * 0.15)) //validObj
|
||||
{
|
||||
//在XY平面内直线拟合
|
||||
//为了防止端部影响，跳过端面数据
|
||||
|
||||
@ -90,6 +90,12 @@ SG_APISHARED_EXPORT SSX_platePoseInfo sx_getLocationPlatePose(
|
||||
const SSG_cornerParam cornerPara,
|
||||
int* errCode);
|
||||
|
||||
//新的计算定位盘中心点位姿（去除了定位盘，只有定位板）
|
||||
SG_APISHARED_EXPORT SSX_platePoseInfo sx_getLocationPlatePose_new(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SSG_cornerParam cornerPara,
|
||||
int* errCode);
|
||||
|
||||
//堵第蚰龰隅弇
|
||||
SG_APISHARED_EXPORT void sx_rodPositioning(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user