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 <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()
|
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 或调试 >“开始执行(不调试)”菜单
|
// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
|
||||||
|
|||||||
@ -146,7 +146,7 @@ SG_APISHARED_EXPORT void wd_getRingArcFeature(
|
|||||||
std::vector< SVzNL3DPosition>& lineData,
|
std::vector< SVzNL3DPosition>& lineData,
|
||||||
int lineIdx,
|
int lineIdx,
|
||||||
const SSG_cornerParam cornerPara,
|
const SSG_cornerParam cornerPara,
|
||||||
double ringArcWidth, //定子的环宽度
|
SVzNLRangeD ringArcWidth, //定子的环宽度
|
||||||
std::vector<SWD_segFeature>& line_ringArcs //环
|
std::vector<SWD_segFeature>& line_ringArcs //环
|
||||||
);
|
);
|
||||||
|
|
||||||
@ -393,6 +393,10 @@ SG_APISHARED_EXPORT SVzNL3DRangeD sg_getScanDataROI_vector(
|
|||||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines
|
std::vector< std::vector<SVzNL3DPosition>>& scanLines
|
||||||
);
|
);
|
||||||
|
|
||||||
|
//计算点云ROI: vecotr格式
|
||||||
|
SG_APISHARED_EXPORT SVzNL3DRangeD wd_getPointCloudROI(
|
||||||
|
std::vector<SVzNL3DPoint>& scanData);
|
||||||
|
|
||||||
//计算点云的ROI和scale: vecotr格式
|
//计算点云的ROI和scale: vecotr格式
|
||||||
SG_APISHARED_EXPORT SWD_pointCloudPara wd_getPointCloudPara(
|
SG_APISHARED_EXPORT SWD_pointCloudPara wd_getPointCloudPara(
|
||||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines);
|
std::vector< std::vector<SVzNL3DPosition>>& scanLines);
|
||||||
|
|||||||
@ -202,6 +202,7 @@ typedef struct
|
|||||||
int endPtIdx;
|
int endPtIdx;
|
||||||
SVzNL3DPoint startPt;
|
SVzNL3DPoint startPt;
|
||||||
SVzNL3DPoint endPt;
|
SVzNL3DPoint endPt;
|
||||||
|
double featureValue;
|
||||||
}SWD_segFeature;
|
}SWD_segFeature;
|
||||||
|
|
||||||
typedef struct
|
typedef struct
|
||||||
|
|||||||
@ -131,6 +131,64 @@ SVzNL3DRangeD sg_getScanDataROI_vector(std::vector< std::vector<SVzNL3DPosition>
|
|||||||
return roi;
|
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格式
|
//计算点云的ROI和scale: vecotr格式
|
||||||
SWD_pointCloudPara wd_getPointCloudPara(std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
SWD_pointCloudPara wd_getPointCloudPara(std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||||||
{
|
{
|
||||||
@ -445,6 +503,7 @@ bool leastSquareParabolaFit(const std::vector<cv::Point2d>& points,
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
//抛物线最小二乘拟合 y=ax^2 + bx + c
|
||||||
bool leastSquareParabolaFitEigen(
|
bool leastSquareParabolaFitEigen(
|
||||||
const std::vector<cv::Point2d>& points,
|
const std::vector<cv::Point2d>& points,
|
||||||
double& a, double& b, double& c,
|
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; // 维度匹配，无报错
|
Eigen::MatrixXd centered = A.rowwise() - centroid_row; // 维度匹配，无报错
|
||||||
|
|
||||||
// 协方差矩阵计算（n-1为无偏估计，工程中也可直接用n）
|
// 协方差矩阵计算（n-1为无偏估计，工程中也可直接用n）
|
||||||
Eigen::Matrix3d cov = centered.transpose() * centered / (points.size() - 1);
|
Eigen::Matrix3d cov = centered.transpose() * centered; // / (points.size() - 1);
|
||||||
// 3. 特征值分解：求协方差矩阵的特征值和特征向量
|
// 3. 特征值分解：求协方差矩阵的特征值和特征向量
|
||||||
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(cov);
|
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(cov);
|
||||||
if (eigensolver.info() != Eigen::Success) {
|
if (eigensolver.info() != Eigen::Success) {
|
||||||
@ -3128,3 +3187,87 @@ bool fitLine3DLeastSquares(const std::vector<SVzNL3DPoint>& points, SVzNL3DPoint
|
|||||||
direction = { dir(0), dir(1), dir(2) };
|
direction = { dir(0), dir(1), dir(2) };
|
||||||
return true;
|
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,
|
std::vector< SVzNL3DPosition>& lineData,
|
||||||
int lineIdx,
|
int lineIdx,
|
||||||
const SSG_cornerParam cornerPara,
|
const SSG_cornerParam cornerPara,
|
||||||
double ringArcWidth, //环宽度
|
SVzNLRangeD ringArcWidth, //环宽度
|
||||||
std::vector<SWD_segFeature>& line_ringArcs //»·
|
std::vector<SWD_segFeature>& line_ringArcs //»·
|
||||||
)
|
)
|
||||||
{
|
{
|
||||||
@ -3588,262 +3588,112 @@ void wd_getRingArcFeature(
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
//搜索拐角极值
|
//逐段寻找连续的负corner段。Arc上的每一个点都是负的corner。
|
||||||
int _state = 0;
|
int vldPtSize = (int)vldPts.size();
|
||||||
int pre_i = -1;
|
int startIdx = -1;
|
||||||
int sEdgePtIdx = -1;
|
int endIdx = -1;
|
||||||
int eEdgePtIdx = -1;
|
double startAngle = 0;
|
||||||
SSG_pntDirAngle* pre_data = NULL;
|
double endAngle = 0;
|
||||||
std::vector< SSG_pntDirAngle> cornerPeakP;
|
for (int segIdx = 0; segIdx < segSize; segIdx++)
|
||||||
std::vector< SSG_pntDirAngle> cornerPeakM;
|
|
||||||
for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++)
|
|
||||||
{
|
{
|
||||||
if (i == 275)
|
int vPtIdxStart = segs[segIdx].start;
|
||||||
int kkk = 1;
|
int vPtIdxEnd = vPtIdxStart + segs[segIdx].len - 1;
|
||||||
SSG_pntDirAngle* curr_data = &corners[i];
|
for (int i = vPtIdxStart; i <= vPtIdxEnd; i++)
|
||||||
if (curr_data->pntIdx < 0)
|
|
||||||
{
|
{
|
||||||
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) //下降
|
else
|
||||||
{
|
{
|
||||||
cornerPeakM.push_back(corners[eEdgePtIdx]);
|
endIdx = i;
|
||||||
|
endAngle = corners[i].forwardAngle;
|
||||||
|
if (i == dataSize - 1)
|
||||||
|
{
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
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
|
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--)
|
|
||||||
{
|
|
||||||
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;
|
if (startIdx >= 0)
|
||||||
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;
|
double totalCorner = abs(endAngle - startAngle);
|
||||||
break;
|
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;
|
||||||
if (true == isPeak)
|
startAngle = 0;
|
||||||
{
|
endAngle = 0;
|
||||||
|
|
||||||
|
|
||||||
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))
|
|
||||||
{
|
|
||||||
SWD_segFeature a_ringFeautre;
|
|
||||||
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;
|
|
||||||
line_ringArcs.push_back(a_ringFeautre);
|
|
||||||
lineFeatures[i].featureType = -1;
|
|
||||||
lineFeatures[i + 1].featureType = -1;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -14,7 +14,8 @@ const char* wd_rodAndBarDetectionVersion(void)
|
|||||||
|
|
||||||
SVzNL3DPoint getArcPeak(
|
SVzNL3DPoint getArcPeak(
|
||||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
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;
|
SVzNL3DPoint arcPeak = scanLines[a_arcFeature.lineIdx][a_arcFeature.startPtIdx].pt3D;
|
||||||
for (int i = a_arcFeature.startPtIdx+1; i <= a_arcFeature.endPtIdx; i++)
|
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 (scanLines[a_arcFeature.lineIdx][i].pt3D.z > 1e-4) //跳开空点
|
||||||
{
|
{
|
||||||
if (arcPeak.z > scanLines[a_arcFeature.lineIdx][i].pt3D.z)
|
if (arcPeak.z > scanLines[a_arcFeature.lineIdx][i].pt3D.z)
|
||||||
|
{
|
||||||
arcPeak = scanLines[a_arcFeature.lineIdx][i].pt3D;
|
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;
|
return arcPeak;
|
||||||
@ -119,12 +177,11 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
|
|||||||
const SSG_cornerParam cornerPara,
|
const SSG_cornerParam cornerPara,
|
||||||
const SSG_outlierFilterParam filterParam,
|
const SSG_outlierFilterParam filterParam,
|
||||||
const SSG_treeGrowParam growParam,
|
const SSG_treeGrowParam growParam,
|
||||||
double rodRidius,
|
double rodDiameter,
|
||||||
std::vector<SSX_hexHeadScrewInfo>& screwInfo,
|
std::vector<SSX_hexHeadScrewInfo>& screwInfo,
|
||||||
int* errCode)
|
int* errCode)
|
||||||
{
|
{
|
||||||
*errCode = 0;
|
*errCode = 0;
|
||||||
memset(&screwInfo, 0, sizeof(SSX_hexHeadScrewInfo));
|
|
||||||
int lineNum = (int)scanLines.size();
|
int lineNum = (int)scanLines.size();
|
||||||
if (lineNum == 0)
|
if (lineNum == 0)
|
||||||
{
|
{
|
||||||
@ -193,7 +250,7 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
|
|||||||
std::vector<std::vector<SWD_segFeature>> arcFeatures;
|
std::vector<std::vector<SWD_segFeature>> arcFeatures;
|
||||||
for (int line = 0; line < lineNum; line++)
|
for (int line = 0; line < lineNum; line++)
|
||||||
{
|
{
|
||||||
if (line == 44)
|
if (line == 329)
|
||||||
int kkk = 1;
|
int kkk = 1;
|
||||||
std::vector<SVzNL3DPosition>& lineData = data_lines[line];
|
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);
|
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||||||
std::vector<SWD_segFeature> line_ringArcs;
|
std::vector<SWD_segFeature> line_ringArcs;
|
||||||
int dataSize = (int)lineData.size();
|
int dataSize = (int)lineData.size();
|
||||||
|
SVzNLRangeD arcWidth;
|
||||||
|
arcWidth.min = rodDiameter / 2;
|
||||||
|
arcWidth.max = rodDiameter * 1.5;
|
||||||
//提取Arc特征
|
//提取Arc特征
|
||||||
wd_getRingArcFeature(
|
wd_getRingArcFeature(
|
||||||
lineData,
|
lineData,
|
||||||
line, //当前扫描线序号
|
line, //当前扫描线序号
|
||||||
cornerPara,
|
cornerPara,
|
||||||
rodRidius / 2, //环宽度
|
arcWidth, //环宽度，以半径为基准，对应60度角
|
||||||
line_ringArcs //环
|
line_ringArcs //环
|
||||||
);
|
);
|
||||||
arcFeatures.push_back(line_ringArcs);
|
arcFeatures.push_back(line_ringArcs);
|
||||||
@ -223,23 +283,75 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
|
|||||||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
int objNum = (int)growTrees.size();
|
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++)
|
for (int i = 0; i < objNum; i++)
|
||||||
{
|
{
|
||||||
//空间直线拟合
|
//空间直线拟合
|
||||||
std::vector<SVzNL3DPoint> fitPoints;
|
std::vector<SVzNL3DPoint> fitPoints;
|
||||||
|
std::vector<SVzNL2DPoint> fit2DPos;
|
||||||
int nodeSize = (int)growTrees[i].treeNodes.size();
|
int nodeSize = (int)growTrees[i].treeNodes.size();
|
||||||
for (int j = 0; j < nodeSize; j++)
|
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);
|
fitPoints.push_back(a_pt);
|
||||||
|
fit2DPos.push_back(arcPos);
|
||||||
}
|
}
|
||||||
if (fitPoints.size() < 12)
|
if (fitPoints.size() < 27)
|
||||||
continue;
|
continue;
|
||||||
//去除头尾各5个点，防止在端部和根部扫描时的数据有干扰
|
//去除头尾各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());
|
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;
|
SVzNL3DPoint P0_center, P1_dir;
|
||||||
bool result = fitLine3DLeastSquares(fitPoints, 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 };
|
SVzNL3DPoint vector2 = { 0, 0, -1.0 };
|
||||||
SSG_planeCalibPara rotatePara = wd_computeRTMatrix( vector1, vector2);
|
SSG_planeCalibPara rotatePara = wd_computeRTMatrix( vector1, vector2);
|
||||||
//
|
//
|
||||||
|
SVzNL3DPoint P0_rotate = _ptRotate(P0_center, rotatePara.planeCalib);
|
||||||
SSG_ROIRectD roi_xoy;
|
SSG_ROIRectD roi_xoy;
|
||||||
roi_xoy.left = P0_center.x - rodRidius * 4; //2D范围
|
roi_xoy.left = P0_rotate.x - rodDiameter * 2; //2D范围
|
||||||
roi_xoy.right = P0_center.x + rodRidius * 4; //2D范围
|
roi_xoy.right = P0_rotate.x + rodDiameter * 2; //2D范围
|
||||||
roi_xoy.top = P0_center.y - rodRidius * 4; //2D范围
|
roi_xoy.top = P0_rotate.y - rodDiameter * 2; //2D范围
|
||||||
roi_xoy.bottom = P0_center.y + rodRidius * 4; //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;
|
std::vector< SVzNL3DPoint> roiProjectionData;
|
||||||
xoyROIProjection(scanLines, rotatePara.planeCalib, roi_xoy, roiProjectionData);
|
xoyROIProjection(data_lines, rotatePara.planeCalib, roi_xoy, roiProjectionData);
|
||||||
//取端面
|
//取端面
|
||||||
SVzNLRangeD zRange = getZRange(roiProjectionData);
|
SVzNLRangeD zRange = getZRange(roiProjectionData);
|
||||||
SVzNLRangeD cutZRange;
|
SVzNLRangeD cutZRange;
|
||||||
cutZRange.min = zRange.min;
|
cutZRange.min = zRange.min;
|
||||||
cutZRange.max = zRange.min + 10.0; //取10mm的端面
|
cutZRange.max = zRange.min + 5.0; //5mm的端面
|
||||||
std::vector<SVzNL3DPoint> surfacePoints;
|
std::vector<SVzNL3DPoint> surfacePoints;
|
||||||
zCutPointClouds(roiProjectionData, cutZRange, 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;
|
projectionCenter.z = zRange.min;
|
||||||
//旋转回原坐标系
|
//旋转回原坐标系
|
||||||
SVzNL3DPoint surfaceCenter = _ptRotate(projectionCenter, rotatePara.invRMatrix);
|
SVzNL3DPoint surfaceCenter = _ptRotate(projectionCenter, rotatePara.invRMatrix);
|
||||||
|
|||||||
@ -22,6 +22,6 @@ SG_APISHARED_EXPORT void sx_hexHeadScrewMeasure(
|
|||||||
const SSG_cornerParam cornerPara,
|
const SSG_cornerParam cornerPara,
|
||||||
const SSG_outlierFilterParam filterParam,
|
const SSG_outlierFilterParam filterParam,
|
||||||
const SSG_treeGrowParam growParam,
|
const SSG_treeGrowParam growParam,
|
||||||
double rodRidius,
|
double rodDiameter,
|
||||||
std::vector<SSX_hexHeadScrewInfo>& screwInfo,
|
std::vector<SSX_hexHeadScrewInfo>& screwInfo,
|
||||||
int* errCode);
|
int* errCode);
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user