rodAndBarDetection version 1.0.0
初始版本
This commit is contained in:
parent
c69f28ca41
commit
0111e2a56a
@ -1,11 +1,398 @@
|
||||
// rodAndBarDetection_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
|
||||
// gasFillingPortPosition_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
|
||||
//
|
||||
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <vector>
|
||||
#include <stdio.h>
|
||||
#include <VZNL_Types.h>
|
||||
#include "direct.h"
|
||||
#include <string>
|
||||
#include "rodAndBarDetection_Export.h"
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <Windows.h>
|
||||
#include <limits>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int r;
|
||||
int g;
|
||||
int b;
|
||||
}SG_color;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int nPointIdx;
|
||||
double x;
|
||||
double y;
|
||||
double z;
|
||||
float r;
|
||||
float g;
|
||||
float b;
|
||||
} SPointXYZRGB;
|
||||
|
||||
void wdReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector<std::vector< SVzNL3DPosition>>& scanData)
|
||||
{
|
||||
std::ifstream inputFile(fileName);
|
||||
std::string linedata;
|
||||
|
||||
if (inputFile.is_open() == false)
|
||||
return;
|
||||
|
||||
std::vector< SVzNL3DPosition> a_line;
|
||||
int ptIdx = 0;
|
||||
while (getline(inputFile, linedata))
|
||||
{
|
||||
if (0 == strncmp("Line_", linedata.c_str(), 5))
|
||||
{
|
||||
int ptSize = (int)a_line.size();
|
||||
if (ptSize > 0)
|
||||
{
|
||||
scanData.push_back(a_line);
|
||||
}
|
||||
a_line.clear();
|
||||
ptIdx = 0;
|
||||
}
|
||||
else if (0 == strncmp("{", linedata.c_str(), 1))
|
||||
{
|
||||
float X, Y, Z;
|
||||
int imageY = 0;
|
||||
float leftX, leftY;
|
||||
float rightX, rightY;
|
||||
sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY);
|
||||
SVzNL3DPosition a_pt;
|
||||
a_pt.pt3D.x = X;
|
||||
a_pt.pt3D.y = Y;
|
||||
a_pt.pt3D.z = Z;
|
||||
a_pt.nPointIdx = ptIdx;
|
||||
ptIdx++;
|
||||
a_line.push_back(a_pt);
|
||||
}
|
||||
}
|
||||
//last line
|
||||
int ptSize = (int)a_line.size();
|
||||
if (ptSize > 0)
|
||||
{
|
||||
scanData.push_back(a_line);
|
||||
a_line.clear();
|
||||
}
|
||||
|
||||
inputFile.close();
|
||||
return;
|
||||
}
|
||||
|
||||
void wd_gridScan_GetROIData(std::vector<std::vector< SVzNL3DPosition>>& scanData, SVzNLRangeD roi_y, std::vector<std::vector< SVzNL3DPosition>>& roiData)
|
||||
{
|
||||
int lineNum = (int)scanData.size();
|
||||
int linePtNum = (int)scanData[0].size();
|
||||
int globalPtStart = INT_MAX;
|
||||
int globalPtEnd = 0;
|
||||
int lineStart = INT_MAX;
|
||||
int lineEnd = 0;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
std::vector< SVzNL3DPosition >& lineData = scanData[line];
|
||||
int ptSize = (int)lineData.size();
|
||||
int vldNum = 0;
|
||||
int ptStart = INT_MAX;
|
||||
int ptEnd = 0;
|
||||
for (int i = 0; i < ptSize; i++)
|
||||
{
|
||||
if (lineData[i].pt3D.z > 1e-4)
|
||||
{
|
||||
if ((lineData[i].pt3D.y < roi_y.min) || (lineData[i].pt3D.y > roi_y.max))
|
||||
lineData[i].pt3D = { 0.0, 0.0, 0.0 };
|
||||
}
|
||||
|
||||
if (lineData[i].pt3D.z > 1e-4)
|
||||
{
|
||||
if (ptStart > i)
|
||||
ptStart = i;
|
||||
ptEnd = i;
|
||||
vldNum++;
|
||||
}
|
||||
}
|
||||
if (vldNum > 0)
|
||||
{
|
||||
if (globalPtStart > ptStart)
|
||||
globalPtStart = ptStart;
|
||||
if (globalPtEnd < ptEnd)
|
||||
globalPtEnd = ptEnd;
|
||||
|
||||
if (lineStart > line)
|
||||
lineStart = line;
|
||||
lineEnd = line;
|
||||
}
|
||||
}
|
||||
int vldLineNum = lineEnd - lineStart + 1;
|
||||
int vldPtNum = globalPtEnd - globalPtStart + 1;
|
||||
|
||||
roiData.resize(vldLineNum);
|
||||
for (int line = 0; line < vldLineNum; line++)
|
||||
{
|
||||
roiData[line].resize(vldPtNum);
|
||||
for (int i = 0; i < vldPtNum; i++)
|
||||
roiData[line][i] = scanData[line + lineStart][i + globalPtStart];
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
void _outputScanDataFile(char* fileName, std::vector<std::vector< SVzNL3DPosition>>& scanData,
|
||||
float lineV, int maxTimeStamp, int clockPerSecond)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
|
||||
int lineNum = (int)scanData.size();
|
||||
sw << "LineNum:" << lineNum << std::endl;
|
||||
sw << "DataType: 0" << std::endl;
|
||||
sw << "ScanSpeed:" << lineV << std::endl;
|
||||
sw << "PointAdjust: 1" << std::endl;
|
||||
sw << "MaxTimeStamp:" << maxTimeStamp << "_" << clockPerSecond << std::endl;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
int nPositionCnt = (int)scanData[line].size();
|
||||
sw << "Line_" << line << "_0_" << nPositionCnt << std::endl;
|
||||
for (int i = 0; i < nPositionCnt; i++)
|
||||
{
|
||||
SVzNL3DPosition& pt3D = scanData[line][i];
|
||||
float x = (float)pt3D.pt3D.x;
|
||||
float y = (float)pt3D.pt3D.y;
|
||||
float z = (float)pt3D.pt3D.z;
|
||||
sw << "{ " << x << "," << y << "," << z << " }-";
|
||||
sw << "{0,0}-{0,0}" << std::endl;
|
||||
}
|
||||
}
|
||||
sw.close();
|
||||
}
|
||||
|
||||
void _outputChanneltInfo(char* fileName, std::vector<SSX_hexHeadScrewInfo>& screwInfo)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
|
||||
char dataStr[250];
|
||||
int objNum = (int)screwInfo.size();
|
||||
for (int i = 0; i < objNum; i++)
|
||||
{
|
||||
sprintf_s(dataStr, 250, "螺杆_%d: center_( %g, %g, %g ), dir_( %g, %g, %g ), anlge_%g",
|
||||
i + 1, screwInfo[i].center.x, screwInfo[i].center.y, screwInfo[i].center.z,
|
||||
screwInfo[i].axialDir.x, screwInfo[i].axialDir.y, screwInfo[i].axialDir.z, screwInfo[i].rotateAngle);
|
||||
sw << dataStr << std::endl;
|
||||
}
|
||||
sw.close();
|
||||
}
|
||||
|
||||
void _outputRGBDScan_RGBD(
|
||||
char* fileName,
|
||||
std::vector<std::vector<SVzNL3DPosition>>& scanLines,
|
||||
std::vector<SSX_hexHeadScrewInfo>& screwInfo
|
||||
)
|
||||
{
|
||||
int lineNum = (int)scanLines.size();
|
||||
std::ofstream sw(fileName);
|
||||
int realLines = lineNum;
|
||||
int objNum = (int)screwInfo.size();
|
||||
if (objNum > 0)
|
||||
realLines += 1;
|
||||
|
||||
sw << "LineNum:" << realLines << std::endl;
|
||||
sw << "DataType: 0" << std::endl;
|
||||
sw << "ScanSpeed: 0" << std::endl;
|
||||
sw << "PointAdjust: 1" << std::endl;
|
||||
sw << "MaxTimeStamp: 0_0" << std::endl;
|
||||
|
||||
int maxLineIndex = 0;
|
||||
int max_stamp = 0;
|
||||
|
||||
SG_color rgb = { 0, 0, 0 };
|
||||
|
||||
SG_color objColor[8] = {
|
||||
{245,222,179},//淡黄色
|
||||
{210,105, 30},//巧克力色
|
||||
{240,230,140},//黄褐色
|
||||
{135,206,235},//天蓝色
|
||||
{250,235,215},//古董白
|
||||
{189,252,201},//薄荷色
|
||||
{221,160,221},//梅红色
|
||||
{188,143,143},//玫瑰红色
|
||||
};
|
||||
int size = 1;
|
||||
int lineIdx = 0;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
int linePtNum = (int)scanLines[line].size();
|
||||
if (linePtNum == 0)
|
||||
continue;
|
||||
|
||||
sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl;
|
||||
lineIdx++;
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
SVzNL3DPosition* pt3D = &scanLines[line][i];
|
||||
if (pt3D->nPointIdx == 1)
|
||||
{
|
||||
rgb = objColor[pt3D->nPointIdx];
|
||||
size = 3;
|
||||
}
|
||||
else if (pt3D->nPointIdx == 2)
|
||||
{
|
||||
rgb = { 250, 0, 0 };
|
||||
size = 5;
|
||||
}
|
||||
|
||||
else //if (pt3D->nPointIdx == 0)
|
||||
{
|
||||
rgb = { 200, 200, 200 };
|
||||
size = 1;
|
||||
}
|
||||
float x = (float)pt3D->pt3D.x;
|
||||
float y = (float)pt3D->pt3D.y;
|
||||
float z = (float)pt3D->pt3D.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
if (objNum > 0)
|
||||
{
|
||||
sw << "Line_" << lineIdx << "_0_" << objNum << std::endl;
|
||||
rgb = { 250, 0, 0 };
|
||||
size = 8;
|
||||
for (int i = 0; i < objNum; i++)
|
||||
{
|
||||
float x = (float)screwInfo[i].center.x;
|
||||
float y = (float)screwInfo[i].center.y;
|
||||
float z = (float)screwInfo[i].center.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
//多输出一个，修正显示工具bug
|
||||
float x = (float)screwInfo[0].center.x;
|
||||
float y = (float)screwInfo[0].center.y;
|
||||
float z = (float)screwInfo[0].center.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
|
||||
//输出法向
|
||||
size = 1;
|
||||
double len = 60;
|
||||
lineIdx = 0;
|
||||
for (int i = 0; i < objNum; i++)
|
||||
{
|
||||
SVzNL3DPoint pt0 = { screwInfo[i].center.x - len * screwInfo[i].axialDir.x,
|
||||
screwInfo[i].center.y - len * screwInfo[i].axialDir.y,
|
||||
screwInfo[i].center.z - len * screwInfo[i].axialDir.z };
|
||||
SVzNL3DPoint pt1 = { screwInfo[i].center.x + len * screwInfo[i].axialDir.x,
|
||||
screwInfo[i].center.y + len * screwInfo[i].axialDir.y,
|
||||
screwInfo[i].center.z + len * screwInfo[i].axialDir.z };
|
||||
//显示法向量
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << pt1.x << "," << pt1.y << "," << pt1.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
lineIdx++;
|
||||
}
|
||||
//多输出一个，修正显示工具bug
|
||||
SVzNL3DPoint pt0 = { screwInfo[0].center.x - len * screwInfo[0].axialDir.x,
|
||||
screwInfo[0].center.y - len * screwInfo[0].axialDir.y,
|
||||
screwInfo[0].center.z - len * screwInfo[0].axialDir.z };
|
||||
SVzNL3DPoint pt1 = { screwInfo[0].center.x + len * screwInfo[0].axialDir.x,
|
||||
screwInfo[0].center.y + len * screwInfo[0].axialDir.y,
|
||||
screwInfo[0].center.z + len * screwInfo[0].axialDir.z };
|
||||
//显示法向量
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << pt1.x << "," << pt1.y << "," << pt1.z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
lineIdx++;
|
||||
}
|
||||
sw.close();
|
||||
}
|
||||
|
||||
#define TEST_GROUP 1
|
||||
int main()
|
||||
{
|
||||
std::cout << "Hello World!\n";
|
||||
const char* dataPath[TEST_GROUP] = {
|
||||
"F:/ShangGu/项目/冠钦项目/螺杆测量/数据/模拟数据/", //0
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[TEST_GROUP] = {
|
||||
{1,4},
|
||||
};
|
||||
|
||||
const char* ver = wd_rodAndBarDetectionVersion();
|
||||
printf("ver:%s\n", ver);
|
||||
|
||||
for (int grp = 0; grp < TEST_GROUP; grp++)
|
||||
{
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
//fidx =7;
|
||||
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();//统计时间
|
||||
|
||||
double rodDiameter = 10.0;
|
||||
|
||||
SSG_cornerParam cornerParam;
|
||||
cornerParam.cornerTh = 60; //45度角
|
||||
cornerParam.scale = rodDiameter/4; // 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;
|
||||
|
||||
SSG_outlierFilterParam filterParam;
|
||||
filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声
|
||||
filterParam.outlierTh = 5;
|
||||
|
||||
SSG_treeGrowParam growParam;
|
||||
growParam.maxLineSkipNum = 10;
|
||||
growParam.yDeviation_max = 20.0;
|
||||
growParam.maxSkipDistance = 20.0;
|
||||
growParam.zDeviation_max = 50.0;//
|
||||
growParam.minLTypeTreeLen = 10; //mm, 螺杆长度
|
||||
growParam.minVTypeTreeLen = 10; //mm
|
||||
|
||||
bool isHorizonScan = true; //true:激光线平行槽道；false:激光线垂直槽道
|
||||
int errCode = 0;
|
||||
std::vector<SSX_hexHeadScrewInfo> screwInfo;
|
||||
sx_hexHeadScrewMeasure(
|
||||
scanLines,
|
||||
isHorizonScan, //true:激光线平行槽道；false:激光线垂直槽道
|
||||
cornerParam,
|
||||
filterParam,
|
||||
growParam,
|
||||
rodDiameter,
|
||||
screwInfo,
|
||||
&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, screwInfo);
|
||||
sprintf_s(_scan_file, "%sresult\\%d_screw_info.txt", dataPath[grp], fidx);
|
||||
_outputChanneltInfo(_scan_file, screwInfo);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
|
||||
|
||||
@ -146,7 +146,7 @@ SG_APISHARED_EXPORT void wd_getRingArcFeature(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
int lineIdx,
|
||||
const SSG_cornerParam cornerPara,
|
||||
double ringArcWidth, //定子的环宽度
|
||||
SVzNLRangeD ringArcWidth, //定子的环宽度
|
||||
std::vector<SWD_segFeature>& line_ringArcs //环
|
||||
);
|
||||
|
||||
@ -393,6 +393,10 @@ SG_APISHARED_EXPORT SVzNL3DRangeD sg_getScanDataROI_vector(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines
|
||||
);
|
||||
|
||||
//计算点云ROI: vecotr格式
|
||||
SG_APISHARED_EXPORT SVzNL3DRangeD wd_getPointCloudROI(
|
||||
std::vector<SVzNL3DPoint>& scanData);
|
||||
|
||||
//计算点云的ROI和scale: vecotr格式
|
||||
SG_APISHARED_EXPORT SWD_pointCloudPara wd_getPointCloudPara(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines);
|
||||
|
||||
@ -202,6 +202,7 @@ typedef struct
|
||||
int endPtIdx;
|
||||
SVzNL3DPoint startPt;
|
||||
SVzNL3DPoint endPt;
|
||||
double featureValue;
|
||||
}SWD_segFeature;
|
||||
|
||||
typedef struct
|
||||
|
||||
@ -131,6 +131,64 @@ SVzNL3DRangeD sg_getScanDataROI_vector(std::vector< std::vector<SVzNL3DPosition>
|
||||
return roi;
|
||||
}
|
||||
|
||||
//计算点云ROI: vecotr格式
|
||||
SVzNL3DRangeD wd_getPointCloudROI(std::vector<SVzNL3DPoint>& scanData)
|
||||
{
|
||||
SVzNL3DRangeD roi;
|
||||
roi.xRange = { 0, -1 };
|
||||
roi.yRange = { 0, -1 };
|
||||
roi.zRange = { 0, -1 };
|
||||
|
||||
int nPositionCnt = (int)scanData.size();
|
||||
for (int i = 0; i < nPositionCnt; i++)
|
||||
{
|
||||
SVzNL3DPoint& pt3D = scanData[i];
|
||||
if (pt3D.z < 1e-4)
|
||||
continue;
|
||||
|
||||
if (roi.xRange.max < roi.xRange.min)
|
||||
{
|
||||
roi.xRange.min = pt3D.x;
|
||||
roi.xRange.max = pt3D.x;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (roi.xRange.min > pt3D.x)
|
||||
roi.xRange.min = pt3D.x;
|
||||
if (roi.xRange.max < pt3D.x)
|
||||
roi.xRange.max = pt3D.x;
|
||||
}
|
||||
//y
|
||||
if (roi.yRange.max < roi.yRange.min)
|
||||
{
|
||||
roi.yRange.min = pt3D.y;
|
||||
roi.yRange.max = pt3D.y;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (roi.yRange.min > pt3D.y)
|
||||
roi.yRange.min = pt3D.y;
|
||||
if (roi.yRange.max < pt3D.y)
|
||||
roi.yRange.max = pt3D.y;
|
||||
}
|
||||
//z
|
||||
if (roi.zRange.max < roi.zRange.min)
|
||||
{
|
||||
roi.zRange.min = pt3D.z;
|
||||
roi.zRange.max = pt3D.z;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (roi.zRange.min > pt3D.z)
|
||||
roi.zRange.min = pt3D.z;
|
||||
if (roi.zRange.max < pt3D.z)
|
||||
roi.zRange.max = pt3D.z;
|
||||
}
|
||||
}
|
||||
|
||||
return roi;
|
||||
}
|
||||
|
||||
//计算点云的ROI和scale: vecotr格式
|
||||
SWD_pointCloudPara wd_getPointCloudPara(std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||||
{
|
||||
@ -445,6 +503,7 @@ bool leastSquareParabolaFit(const std::vector<cv::Point2d>& points,
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
//抛物线最小二乘拟合 y=ax^2 + bx + c
|
||||
bool leastSquareParabolaFitEigen(
|
||||
const std::vector<cv::Point2d>& points,
|
||||
double& a, double& b, double& c,
|
||||
@ -3112,7 +3171,7 @@ bool fitLine3DLeastSquares(const std::vector<SVzNL3DPoint>& points, SVzNL3DPoint
|
||||
Eigen::MatrixXd centered = A.rowwise() - centroid_row; // 维度匹配，无报错
|
||||
|
||||
// 协方差矩阵计算（n-1为无偏估计，工程中也可直接用n）
|
||||
Eigen::Matrix3d cov = centered.transpose() * centered / (points.size() - 1);
|
||||
Eigen::Matrix3d cov = centered.transpose() * centered; // / (points.size() - 1);
|
||||
// 3. 特征值分解：求协方差矩阵的特征值和特征向量
|
||||
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(cov);
|
||||
if (eigensolver.info() != Eigen::Success) {
|
||||
@ -3128,3 +3187,87 @@ bool fitLine3DLeastSquares(const std::vector<SVzNL3DPoint>& points, SVzNL3DPoint
|
||||
direction = { dir(0), dir(1), dir(2) };
|
||||
return true;
|
||||
}
|
||||
|
||||
#if 0
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include <Eigen/Dense>
|
||||
#include <Eigen/SVD>
|
||||
|
||||
// Define a struct for 3D points
|
||||
struct Point3D {
|
||||
double x, y, z;
|
||||
};
|
||||
|
||||
// Function to perform 3D line fitting using SVD
|
||||
void fitLine3D(const std::vector<Point3D>& points, Eigen::Vector3d& centroid, Eigen::Vector3d& direction) {
|
||||
int n = points.size();
|
||||
if (n < 2) {
|
||||
std::cerr << "Need at least 2 points to fit a line." << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
// 1. Calculate Centroid
|
||||
centroid.setZero();
|
||||
for (const auto& p : points) {
|
||||
centroid += Eigen::Vector3d(p.x, p.y, p.z);
|
||||
}
|
||||
centroid /= n;
|
||||
|
||||
// 2. Center the data and build the data matrix
|
||||
Eigen::MatrixXd data_matrix(n, 3);
|
||||
for (int i = 0; i < n; ++i) {
|
||||
data_matrix.row(i) << points[i].x - centroid(0),
|
||||
points[i].y - centroid(1),
|
||||
points[i].z - centroid(2);
|
||||
}
|
||||
|
||||
// 3. Apply SVD
|
||||
// We compute the SVD of the centered data matrix
|
||||
Eigen::JacobiSVD<Eigen::MatrixXd> svd(data_matrix, Eigen::ComputeThinV);
|
||||
|
||||
// 4. Extract the direction vector
|
||||
// The right singular vector corresponding to the largest singular value (first column of V)
|
||||
// gives the direction of the best-fit line.
|
||||
direction = svd.matrixV().col(0);
|
||||
}
|
||||
|
||||
int main() {
|
||||
// Sample data points
|
||||
std::vector<Point3D> points = {
|
||||
{1.0, 2.0, 3.0},
|
||||
{2.0, 3.0, 4.0},
|
||||
{3.0, 4.0, 5.0},
|
||||
{4.0, 5.0, 6.0},
|
||||
{5.0, 6.0, 7.0}
|
||||
};
|
||||
|
||||
Eigen::Vector3d centroid;
|
||||
Eigen::Vector3d direction;
|
||||
|
||||
fitLine3D(points, centroid, direction);
|
||||
|
||||
std::cout << "Centroid (point on the line): " << centroid.transpose() << std::endl;
|
||||
std::cout << "Direction vector of the line: " << direction.transpose() << std::endl;
|
||||
std::cout << "Equation of the line: P(t) = Centroid + t * Direction" << std::endl;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
template<class Vector3>
|
||||
std::pair < Vector3, Vector3 > best_line_from_points(const std::vector<Vector3>& c)
|
||||
{
|
||||
// copy coordinates to matrix in Eigen format
|
||||
size_t num_atoms = c.size();
|
||||
Eigen::Matrix< Vector3::Scalar, Eigen::Dynamic, Eigen::Dynamic > centers(num_atoms, 3);
|
||||
for (size_t i = 0; i < num_atoms; ++i) centers.row(i) = c[i];
|
||||
|
||||
Vector3 origin = centers.colwise().mean();
|
||||
Eigen::MatrixXd centered = centers.rowwise() - origin.transpose();
|
||||
Eigen::MatrixXd cov = centered.adjoint() * centered;
|
||||
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eig(cov);
|
||||
Vector3 axis = eig.eigenvectors().col(2).normalized();
|
||||
|
||||
return std::make_pair(origin, axis);
|
||||
}
|
||||
#endif
|
||||
@ -3443,7 +3443,7 @@ void wd_getRingArcFeature(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
int lineIdx,
|
||||
const SSG_cornerParam cornerPara,
|
||||
double ringArcWidth, //环宽度
|
||||
SVzNLRangeD ringArcWidth, //环宽度
|
||||
std::vector<SWD_segFeature>& line_ringArcs //»·
|
||||
)
|
||||
{
|
||||
@ -3588,262 +3588,112 @@ void wd_getRingArcFeature(
|
||||
}
|
||||
}
|
||||
|
||||
//搜索拐角极值
|
||||
int _state = 0;
|
||||
int pre_i = -1;
|
||||
int sEdgePtIdx = -1;
|
||||
int eEdgePtIdx = -1;
|
||||
SSG_pntDirAngle* pre_data = NULL;
|
||||
std::vector< SSG_pntDirAngle> cornerPeakP;
|
||||
std::vector< SSG_pntDirAngle> cornerPeakM;
|
||||
for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++)
|
||||
//逐段寻找连续的负corner段。Arc上的每一个点都是负的corner。
|
||||
int vldPtSize = (int)vldPts.size();
|
||||
int startIdx = -1;
|
||||
int endIdx = -1;
|
||||
double startAngle = 0;
|
||||
double endAngle = 0;
|
||||
for (int segIdx = 0; segIdx < segSize; segIdx++)
|
||||
{
|
||||
if (i == 275)
|
||||
int kkk = 1;
|
||||
SSG_pntDirAngle* curr_data = &corners[i];
|
||||
if (curr_data->pntIdx < 0)
|
||||
int vPtIdxStart = segs[segIdx].start;
|
||||
int vPtIdxEnd = vPtIdxStart + segs[segIdx].len - 1;
|
||||
for (int i = vPtIdxStart; i <= vPtIdxEnd; i++)
|
||||
{
|
||||
if (i == i_max - 1) //最后一个
|
||||
if (corners[i].corner < 0)
|
||||
{
|
||||
if (1 == _state) //上升
|
||||
if (startIdx < 0)
|
||||
{
|
||||
cornerPeakP.push_back(corners[eEdgePtIdx]);
|
||||
startIdx = i;
|
||||
endIdx = i;
|
||||
startAngle = corners[i].backwardAngle;
|
||||
endAngle = corners[i].forwardAngle;
|
||||
}
|
||||
else if (2 == _state) //下降
|
||||
{
|
||||
cornerPeakM.push_back(corners[eEdgePtIdx]);
|
||||
}
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
if (NULL == pre_data)
|
||||
{
|
||||
sEdgePtIdx = i;
|
||||
eEdgePtIdx = i;
|
||||
pre_data = curr_data;
|
||||
pre_i = i;
|
||||
continue;
|
||||
}
|
||||
|
||||
eEdgePtIdx = i;
|
||||
double cornerDiff = curr_data->corner - pre_data->corner;
|
||||
switch (_state)
|
||||
{
|
||||
case 0: //初态
|
||||
if (cornerDiff < 0) //下降
|
||||
{
|
||||
_state = 2;
|
||||
}
|
||||
else if (cornerDiff > 0) //上升
|
||||
{
|
||||
_state = 1;
|
||||
}
|
||||
break;
|
||||
case 1: //上升
|
||||
if (cornerDiff < 0) //下降
|
||||
{
|
||||
if(pre_data->corner > 0)
|
||||
cornerPeakP.push_back(*pre_data);
|
||||
_state = 2;
|
||||
}
|
||||
break;
|
||||
case 2: //下降
|
||||
if (cornerDiff > 0) // 上升
|
||||
{
|
||||
if(pre_data->corner < 0)
|
||||
cornerPeakM.push_back(*pre_data);
|
||||
_state = 1;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
_state = 0;
|
||||
break;
|
||||
}
|
||||
pre_data = curr_data;
|
||||
pre_i = i;
|
||||
}
|
||||
|
||||
std::vector<SSG_basicFeature1D> lineFeatures;
|
||||
//极小值点（峰顶）
|
||||
//极值比较，在尺度窗口下寻找局部极值点
|
||||
double square_distTh = 16 * cornerPara.scale * cornerPara.scale; //2倍的cornerScale。
|
||||
for (int i = 0, i_max = (int)cornerPeakP.size(); i < i_max; i++)
|
||||
{
|
||||
if (cornerPeakP[i].corner < cornerPara.cornerTh)
|
||||
continue;
|
||||
|
||||
bool isPeak = true;
|
||||
//向前搜索
|
||||
int cornerPtIdx = cornerPeakP[i].pntIdx;
|
||||
for (int j = i - 1; j >= 0; j--)
|
||||
{
|
||||
int prePtIdx = cornerPeakP[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 (cornerPeakP[i].corner < cornerPeakP[j].corner)
|
||||
{
|
||||
isPeak = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//向后搜索
|
||||
if (true == isPeak)
|
||||
{
|
||||
cornerPtIdx = cornerPeakP[i].pntIdx;
|
||||
for (int j = i + 1; j < i_max; j++)
|
||||
{
|
||||
int postPtIdx = cornerPeakP[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 (cornerPeakP[i].corner < cornerPeakP[j].corner)
|
||||
{
|
||||
isPeak = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (true == isPeak)
|
||||
{
|
||||
|
||||
|
||||
SSG_basicFeature1D a_feature;
|
||||
if ((abs(cornerPeakP[i].backwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakP[i].forwardAngle) > cornerPara.jumpCornerTh_2))
|
||||
a_feature.featureType = LINE_FEATURE_L_JUMP_L2H;
|
||||
else if ((abs(cornerPeakP[i].forwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakP[i].backwardAngle) > cornerPara.jumpCornerTh_2))
|
||||
a_feature.featureType = LINE_FEATURE_L_JUMP_H2L;
|
||||
else
|
||||
a_feature.featureType = LINE_FEATURE_CORNER_V;
|
||||
|
||||
a_feature.featureValue = cornerPeakP[i].corner;
|
||||
a_feature.jumpPos = vldPts[cornerPtIdx].pt3D;
|
||||
a_feature.jumpPos2D = { 0, vldPts[cornerPtIdx].nPointIdx };
|
||||
lineFeatures.push_back(a_feature);
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0, i_max = (int)cornerPeakM.size(); i < i_max; i++)
|
||||
{
|
||||
if (cornerPeakM[i].corner > -cornerPara.cornerTh)
|
||||
continue;
|
||||
|
||||
bool isPeak = true;
|
||||
//向前搜索
|
||||
int cornerPtIdx = cornerPeakM[i].pntIdx;
|
||||
for (int j = i - 1; j >= 0; j--)
|
||||
endIdx = i;
|
||||
endAngle = corners[i].forwardAngle;
|
||||
if (i == dataSize - 1)
|
||||
{
|
||||
int prePtIdx = cornerPeakM[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 (cornerPeakM[i].corner > cornerPeakM[j].corner)
|
||||
{
|
||||
isPeak = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//向后搜索
|
||||
if (true == isPeak)
|
||||
{
|
||||
cornerPtIdx = cornerPeakM[i].pntIdx;
|
||||
for (int j = i + 1; j < i_max; j++)
|
||||
{
|
||||
int postPtIdx = cornerPeakM[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 (cornerPeakM[i].corner > cornerPeakM[j].corner)
|
||||
{
|
||||
isPeak = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (true == isPeak)
|
||||
{
|
||||
|
||||
|
||||
SSG_basicFeature1D a_feature;
|
||||
if ((abs(cornerPeakM[i].backwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakM[i].forwardAngle) > cornerPara.jumpCornerTh_2))
|
||||
a_feature.featureType = LINE_FEATURE_L_JUMP_L2H;
|
||||
else if ((abs(cornerPeakM[i].forwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakM[i].backwardAngle) > cornerPara.jumpCornerTh_2))
|
||||
a_feature.featureType = LINE_FEATURE_L_JUMP_H2L;
|
||||
else
|
||||
a_feature.featureType = LINE_FEATURE_CORNER_V;
|
||||
|
||||
a_feature.featureValue = cornerPeakM[i].corner;
|
||||
a_feature.jumpPos = vldPts[cornerPtIdx].pt3D;
|
||||
a_feature.jumpPos2D = { 0, vldPts[cornerPtIdx].nPointIdx };
|
||||
lineFeatures.push_back(a_feature);
|
||||
}
|
||||
}
|
||||
|
||||
//添加开始和结束边界
|
||||
for (int i = 0, i_max = (int)segs.size(); i < i_max; i++)
|
||||
{
|
||||
int idx_1 = segs[i].start;
|
||||
int idx_2 = segs[i].start + segs[i].len - 1;
|
||||
|
||||
SSG_basicFeature1D an_edge;
|
||||
memset(&an_edge, 0, sizeof(SSG_basicFeature1D));
|
||||
//头部
|
||||
an_edge.featureType = LINE_FEATURE_LINE_ENDING_0;
|
||||
int ptIdx = vldPts[idx_1].nPointIdx;
|
||||
an_edge.jumpPos2D = { 0, ptIdx };
|
||||
an_edge.jumpPos = lineData[ptIdx].pt3D;
|
||||
lineFeatures.push_back(an_edge);
|
||||
//尾部
|
||||
an_edge.featureType = LINE_FEATURE_LINE_ENDING_1;
|
||||
ptIdx = vldPts[idx_2].nPointIdx;
|
||||
an_edge.jumpPos2D = { 0, ptIdx };
|
||||
an_edge.jumpPos = lineData[ptIdx].pt3D;
|
||||
lineFeatures.push_back(an_edge);
|
||||
}
|
||||
|
||||
//排序：按序号排序
|
||||
std::sort(lineFeatures.begin(), lineFeatures.end(), compareByPtIdx);
|
||||
//查找符合宽度的定子环
|
||||
for (int i = 0, i_max = (int)lineFeatures.size(); i < i_max-1; i++)
|
||||
{
|
||||
if (lineFeatures[i].featureType < 0)
|
||||
continue;
|
||||
|
||||
bool pairing = false;
|
||||
if (lineFeatures[i].featureType == LINE_FEATURE_LINE_ENDING_0)
|
||||
{
|
||||
if ((lineFeatures[i + 1].featureType == LINE_FEATURE_LINE_ENDING_1) ||
|
||||
((lineFeatures[i + 1].featureType == LINE_FEATURE_L_JUMP_L2H)&&(lineFeatures[i + 1].featureValue < 0)) )
|
||||
pairing = true;
|
||||
}
|
||||
else if ( (lineFeatures[i].featureType == LINE_FEATURE_L_JUMP_H2L) && (lineFeatures[i].featureValue <0))
|
||||
{
|
||||
if ((lineFeatures[i + 1].featureType == LINE_FEATURE_LINE_ENDING_1) ||
|
||||
((lineFeatures[i + 1].featureType == LINE_FEATURE_L_JUMP_L2H) && (lineFeatures[i + 1].featureValue < 0)))
|
||||
pairing = true;
|
||||
}
|
||||
if (true == pairing)
|
||||
{
|
||||
double deltaY = abs(lineFeatures[i].jumpPos.y - lineFeatures[i + 1].jumpPos.y);
|
||||
double deltaZ = abs(lineFeatures[i].jumpPos.z - lineFeatures[i + 1].jumpPos.z);
|
||||
double th1 = ringArcWidth * 0.5;
|
||||
double th2 = ringArcWidth * 1.25;
|
||||
if ((deltaY >= th1) && (deltaY < th2) && (deltaZ < ringArcWidth * 0.5))
|
||||
double totalCorner = abs(endAngle - startAngle);
|
||||
double len = sqrt(pow(vldPts[startIdx].pt3D.y - vldPts[endIdx].pt3D.y, 2) +
|
||||
pow(vldPts[startIdx].pt3D.z - vldPts[endIdx].pt3D.z, 2));
|
||||
if ((totalCorner > cornerPara.cornerTh) && (len >= ringArcWidth.min) &&
|
||||
(len <= ringArcWidth.max))
|
||||
{
|
||||
int preIdx = startIdx - 1;
|
||||
if (preIdx < vPtIdxStart)
|
||||
preIdx = vPtIdxStart;
|
||||
int postIdx = endIdx + 1;
|
||||
if (postIdx > vPtIdxEnd)
|
||||
postIdx = vPtIdxEnd;
|
||||
//判断整个seg是不是Arc
|
||||
SWD_segFeature a_ringFeautre;
|
||||
memset(&a_ringFeautre, 0, sizeof(SWD_segFeature));
|
||||
a_ringFeautre.lineIdx = lineIdx;
|
||||
a_ringFeautre.startPt = lineFeatures[i].jumpPos;
|
||||
a_ringFeautre.endPt = lineFeatures[i + 1].jumpPos;
|
||||
a_ringFeautre.startPtIdx = lineFeatures[i].jumpPos2D.y;
|
||||
a_ringFeautre.endPtIdx = lineFeatures[i+1].jumpPos2D.y;
|
||||
if ((corners[preIdx].pntIdx < 0) && (corners[postIdx].pntIdx < 0))
|
||||
{
|
||||
a_ringFeautre.startPt = vldPts[vPtIdxStart].pt3D;
|
||||
a_ringFeautre.endPt = vldPts[vPtIdxEnd].pt3D;
|
||||
a_ringFeautre.startPtIdx = vldPts[vPtIdxStart].nPointIdx;
|
||||
a_ringFeautre.endPtIdx = vldPts[vPtIdxEnd].nPointIdx;
|
||||
}
|
||||
else
|
||||
{
|
||||
a_ringFeautre.startPt = vldPts[startIdx].pt3D;
|
||||
a_ringFeautre.endPt = vldPts[endIdx].pt3D;
|
||||
a_ringFeautre.startPtIdx = vldPts[startIdx].nPointIdx;
|
||||
a_ringFeautre.endPtIdx = vldPts[endIdx].nPointIdx;
|
||||
}
|
||||
a_ringFeautre.featureValue = totalCorner;
|
||||
line_ringArcs.push_back(a_ringFeautre);
|
||||
lineFeatures[i].featureType = -1;
|
||||
lineFeatures[i + 1].featureType = -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (startIdx >= 0)
|
||||
{
|
||||
double totalCorner = abs(endAngle - startAngle);
|
||||
double len = sqrt(pow(vldPts[startIdx].pt3D.y - vldPts[endIdx].pt3D.y, 2) +
|
||||
pow(vldPts[startIdx].pt3D.z - vldPts[endIdx].pt3D.z, 2));
|
||||
if ((totalCorner > cornerPara.cornerTh) && (len >= ringArcWidth.min) &&
|
||||
(len <= ringArcWidth.max))
|
||||
{
|
||||
int preIdx = startIdx - 1;
|
||||
if (preIdx < vPtIdxStart)
|
||||
preIdx = vPtIdxStart;
|
||||
int postIdx = endIdx + 1;
|
||||
if (postIdx > vPtIdxEnd)
|
||||
postIdx = vPtIdxEnd;
|
||||
//判断整个seg是不是Arc
|
||||
SWD_segFeature a_ringFeautre;
|
||||
memset(&a_ringFeautre, 0, sizeof(SWD_segFeature));
|
||||
a_ringFeautre.lineIdx = lineIdx;
|
||||
if ((corners[preIdx].pntIdx < 0) && (corners[postIdx].pntIdx < 0))
|
||||
{
|
||||
a_ringFeautre.startPt = vldPts[vPtIdxStart].pt3D;
|
||||
a_ringFeautre.endPt = vldPts[vPtIdxEnd].pt3D;
|
||||
a_ringFeautre.startPtIdx = vldPts[vPtIdxStart].nPointIdx;
|
||||
a_ringFeautre.endPtIdx = vldPts[vPtIdxEnd].nPointIdx;
|
||||
}
|
||||
else
|
||||
{
|
||||
a_ringFeautre.startPt = vldPts[startIdx].pt3D;
|
||||
a_ringFeautre.endPt = vldPts[endIdx].pt3D;
|
||||
a_ringFeautre.startPtIdx = vldPts[startIdx].nPointIdx;
|
||||
a_ringFeautre.endPtIdx = vldPts[endIdx].nPointIdx;
|
||||
}
|
||||
a_ringFeautre.featureValue = totalCorner;
|
||||
line_ringArcs.push_back(a_ringFeautre);
|
||||
}
|
||||
|
||||
}
|
||||
startIdx = -1;
|
||||
endIdx = -1;
|
||||
startAngle = 0;
|
||||
endAngle = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -14,7 +14,8 @@ const char* wd_rodAndBarDetectionVersion(void)
|
||||
|
||||
SVzNL3DPoint getArcPeak(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
SWD_segFeature & a_arcFeature)
|
||||
SWD_segFeature & a_arcFeature,
|
||||
SVzNL2DPoint& arcPos)
|
||||
{
|
||||
SVzNL3DPoint arcPeak = scanLines[a_arcFeature.lineIdx][a_arcFeature.startPtIdx].pt3D;
|
||||
for (int i = a_arcFeature.startPtIdx+1; i <= a_arcFeature.endPtIdx; i++)
|
||||
@ -22,7 +23,64 @@ SVzNL3DPoint getArcPeak(
|
||||
if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4) //跳开空点
|
||||
{
|
||||
if (arcPeak.z > scanLines[a_arcFeature.lineIdx][i].pt3D.z)
|
||||
{
|
||||
arcPeak = scanLines[a_arcFeature.lineIdx][i].pt3D;
|
||||
arcPos = { a_arcFeature.lineIdx , i };
|
||||
}
|
||||
}
|
||||
}
|
||||
return arcPeak;
|
||||
}
|
||||
|
||||
SVzNL3DPoint getArcPeak_parabolaFitting(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
SWD_segFeature& a_arcFeature,
|
||||
SVzNL2DPoint& arcPos)
|
||||
{
|
||||
std::vector<cv::Point2d> points;
|
||||
for (int i = a_arcFeature.startPtIdx + 1; i <= a_arcFeature.endPtIdx; i++)
|
||||
{
|
||||
if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4) //跳开空点
|
||||
{
|
||||
cv::Point2d a_pt2D;
|
||||
if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4)
|
||||
{
|
||||
a_pt2D.x = scanLines[a_arcFeature.lineIdx][i].pt3D.y;
|
||||
a_pt2D.y = scanLines[a_arcFeature.lineIdx][i].pt3D.z;
|
||||
points.push_back(a_pt2D);
|
||||
}
|
||||
}
|
||||
}
|
||||
double a, b, c, mse, max_err;
|
||||
//抛物线最小二乘拟合 y = ax ^ 2 + bx + c
|
||||
bool result = leastSquareParabolaFitEigen(
|
||||
points,
|
||||
a, b, c,
|
||||
mse, max_err);
|
||||
double yP = -b / (2 * a);
|
||||
//寻找与yP最近的点作为Peak点
|
||||
SVzNL3DPoint arcPeak;
|
||||
double minDist = -1;
|
||||
for (int i = a_arcFeature.startPtIdx + 1; i <= a_arcFeature.endPtIdx; i++)
|
||||
{
|
||||
if (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4) //跳开空点
|
||||
{
|
||||
double dist = abs(scanLines[a_arcFeature.lineIdx][i].pt3D.y - yP);
|
||||
if (minDist < 0)
|
||||
{
|
||||
minDist = dist;
|
||||
arcPeak = scanLines[a_arcFeature.lineIdx][i].pt3D;
|
||||
arcPos = { a_arcFeature.lineIdx , i };
|
||||
}
|
||||
else
|
||||
{
|
||||
if(minDist > dist)
|
||||
{
|
||||
minDist = dist;
|
||||
arcPeak = scanLines[a_arcFeature.lineIdx][i].pt3D;
|
||||
arcPos = { a_arcFeature.lineIdx , i };
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return arcPeak;
|
||||
@ -119,12 +177,11 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
|
||||
const SSG_cornerParam cornerPara,
|
||||
const SSG_outlierFilterParam filterParam,
|
||||
const SSG_treeGrowParam growParam,
|
||||
double rodRidius,
|
||||
double rodDiameter,
|
||||
std::vector<SSX_hexHeadScrewInfo>& screwInfo,
|
||||
int* errCode)
|
||||
{
|
||||
*errCode = 0;
|
||||
memset(&screwInfo, 0, sizeof(SSX_hexHeadScrewInfo));
|
||||
int lineNum = (int)scanLines.size();
|
||||
if (lineNum == 0)
|
||||
{
|
||||
@ -193,7 +250,7 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
|
||||
std::vector<std::vector<SWD_segFeature>> arcFeatures;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
if (line == 44)
|
||||
if (line == 329)
|
||||
int kkk = 1;
|
||||
std::vector<SVzNL3DPosition>& lineData = data_lines[line];
|
||||
|
||||
@ -201,12 +258,15 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
|
||||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||||
std::vector<SWD_segFeature> line_ringArcs;
|
||||
int dataSize = (int)lineData.size();
|
||||
SVzNLRangeD arcWidth;
|
||||
arcWidth.min = rodDiameter / 2;
|
||||
arcWidth.max = rodDiameter * 1.5;
|
||||
//提取Arc特征
|
||||
wd_getRingArcFeature(
|
||||
lineData,
|
||||
line, //当前扫描线序号
|
||||
cornerPara,
|
||||
rodRidius / 2, //环宽度
|
||||
arcWidth, //环宽度，以半径为基准，对应60度角
|
||||
line_ringArcs //环
|
||||
);
|
||||
arcFeatures.push_back(line_ringArcs);
|
||||
@ -223,23 +283,75 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
|
||||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||||
return;
|
||||
}
|
||||
|
||||
int objNum = (int)growTrees.size();
|
||||
|
||||
//置标志，用于debug
|
||||
for (int i = 0; i < objNum; i++)
|
||||
{
|
||||
int nodeNum = (int)growTrees[i].treeNodes.size();
|
||||
for (int j = 0; j < nodeNum; j++)
|
||||
{
|
||||
int lineIdx, ptIdx;
|
||||
if (false == isHorizonScan)
|
||||
{
|
||||
lineIdx = growTrees[i].treeNodes[j].lineIdx;
|
||||
for (int m = growTrees[i].treeNodes[j].startPtIdx; m <= growTrees[i].treeNodes[j].endPtIdx; m++)
|
||||
{
|
||||
ptIdx = m;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ptIdx = growTrees[i].treeNodes[j].lineIdx;
|
||||
for (int m = growTrees[i].treeNodes[j].startPtIdx; m <= growTrees[i].treeNodes[j].endPtIdx; m++)
|
||||
{
|
||||
lineIdx = m;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//逐个目标处理
|
||||
for (int i = 0; i < objNum; i++)
|
||||
{
|
||||
//空间直线拟合
|
||||
std::vector<SVzNL3DPoint> fitPoints;
|
||||
std::vector<SVzNL2DPoint> fit2DPos;
|
||||
int nodeSize = (int)growTrees[i].treeNodes.size();
|
||||
for (int j = 0; j < nodeSize; j++)
|
||||
{
|
||||
SVzNL3DPoint a_pt = getArcPeak(data_lines, growTrees[i].treeNodes[j]);
|
||||
SVzNL2DPoint arcPos;
|
||||
SVzNL3DPoint a_pt = getArcPeak_parabolaFitting(data_lines, growTrees[i].treeNodes[j], arcPos);
|
||||
//SVzNL3DPoint a_pt = getArcPeak(data_lines, growTrees[i].treeNodes[j], arcPos);
|
||||
fitPoints.push_back(a_pt);
|
||||
fit2DPos.push_back(arcPos);
|
||||
}
|
||||
if (fitPoints.size() < 12)
|
||||
if (fitPoints.size() < 27)
|
||||
continue;
|
||||
//去除头尾各5个点，防止在端部和根部扫描时的数据有干扰
|
||||
fitPoints.erase(fitPoints.begin(), fitPoints.begin() + 5);
|
||||
fitPoints.erase(fitPoints.begin(), fitPoints.begin() + 10);
|
||||
fit2DPos.erase(fit2DPos.begin(), fit2DPos.begin() + 10);
|
||||
fitPoints.erase(fitPoints.end() - 5, fitPoints.end());
|
||||
fit2DPos.erase(fit2DPos.end() - 5, fit2DPos.end());
|
||||
//置标志
|
||||
for (int j = 0; j < (int)fit2DPos.size(); j++)
|
||||
{
|
||||
int lineIdx, ptIdx;
|
||||
if (false == isHorizonScan)
|
||||
{
|
||||
lineIdx = fit2DPos[j].x;
|
||||
ptIdx = fit2DPos[j].y;
|
||||
}
|
||||
else
|
||||
{
|
||||
lineIdx = fit2DPos[j].y;
|
||||
ptIdx = fit2DPos[j].x;
|
||||
}
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 2;
|
||||
}
|
||||
|
||||
//拟合
|
||||
SVzNL3DPoint P0_center, P1_dir;
|
||||
bool result = fitLine3DLeastSquares(fitPoints, P0_center, P1_dir);
|
||||
@ -251,22 +363,35 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
|
||||
SVzNL3DPoint vector2 = { 0, 0, -1.0 };
|
||||
SSG_planeCalibPara rotatePara = wd_computeRTMatrix( vector1, vector2);
|
||||
//
|
||||
SVzNL3DPoint P0_rotate = _ptRotate(P0_center, rotatePara.planeCalib);
|
||||
SSG_ROIRectD roi_xoy;
|
||||
roi_xoy.left = P0_center.x - rodRidius * 4; //2D范围
|
||||
roi_xoy.right = P0_center.x + rodRidius * 4; //2D范围
|
||||
roi_xoy.top = P0_center.y - rodRidius * 4; //2D范围
|
||||
roi_xoy.bottom = P0_center.y + rodRidius * 4; //2D范围
|
||||
roi_xoy.left = P0_rotate.x - rodDiameter * 2; //2D范围
|
||||
roi_xoy.right = P0_rotate.x + rodDiameter * 2; //2D范围
|
||||
roi_xoy.top = P0_rotate.y - rodDiameter * 2; //2D范围
|
||||
roi_xoy.bottom = P0_rotate.y + rodDiameter * 2; //2D范围
|
||||
|
||||
#if 1
|
||||
std::vector< SVzNL3DPoint> verifyData;
|
||||
for (int m = 0; m < (int)fitPoints.size(); m++)
|
||||
{
|
||||
SVzNL3DPoint rPt = _ptRotate(fitPoints[m], rotatePara.planeCalib);
|
||||
verifyData.push_back(rPt);
|
||||
}
|
||||
#endif
|
||||
std::vector< SVzNL3DPoint> roiProjectionData;
|
||||
xoyROIProjection(scanLines, rotatePara.planeCalib, roi_xoy, roiProjectionData);
|
||||
xoyROIProjection(data_lines, rotatePara.planeCalib, roi_xoy, roiProjectionData);
|
||||
//取端面
|
||||
SVzNLRangeD zRange = getZRange(roiProjectionData);
|
||||
SVzNLRangeD cutZRange;
|
||||
cutZRange.min = zRange.min;
|
||||
cutZRange.max = zRange.min + 10.0; //取10mm的端面
|
||||
cutZRange.max = zRange.min + 5.0; //5mm的端面
|
||||
std::vector<SVzNL3DPoint> surfacePoints;
|
||||
zCutPointClouds(roiProjectionData, cutZRange, surfacePoints);
|
||||
//计算中心点
|
||||
SVzNL3DPoint projectionCenter = getXoYCentroid(surfacePoints);
|
||||
SVzNL3DPoint projectionCenter;// = getXoYCentroid(surfacePoints);
|
||||
SVzNL3DRangeD roi3D = wd_getPointCloudROI(surfacePoints);
|
||||
projectionCenter.x = (roi3D.xRange.min + roi3D.xRange.max) / 2;
|
||||
projectionCenter.y = (roi3D.yRange.min + roi3D.yRange.max) / 2;
|
||||
projectionCenter.z = zRange.min;
|
||||
//旋转回原坐标系
|
||||
SVzNL3DPoint surfaceCenter = _ptRotate(projectionCenter, rotatePara.invRMatrix);
|
||||
|
||||
@ -22,6 +22,6 @@ SG_APISHARED_EXPORT void sx_hexHeadScrewMeasure(
|
||||
const SSG_cornerParam cornerPara,
|
||||
const SSG_outlierFilterParam filterParam,
|
||||
const SSG_treeGrowParam growParam,
|
||||
double rodRidius,
|
||||
double rodDiameter,
|
||||
std::vector<SSX_hexHeadScrewInfo>& screwInfo,
|
||||
int* errCode);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user