Merge branch 'main' of http://gitea.mnutil.com/jerryzeng/algoLib
This commit is contained in:
杰仔
commit
8da9b08bad
@ -268,9 +268,28 @@ SSX_BQAssemblyInfo _readPoseInfo(char* fileName)
|
|||||||
return pose;
|
return pose;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// 检测并跳过UTF-8 BOM头的工具函数
|
||||||
|
void skipUTF8BOM(std::ifstream& file) {
|
||||||
|
// 存储BOM的3个字节（初始化为0）
|
||||||
|
char bom[3] = { 0 };
|
||||||
|
// 读取文件前3个字节
|
||||||
|
file.read(bom, 3);
|
||||||
|
|
||||||
|
// 检测是否是UTF-8 BOM（0xEF 0xBB 0xBF）
|
||||||
|
bool isBOM = (bom[0] == (char)0xEF) &&
|
||||||
|
(bom[1] == (char)0xBB) &&
|
||||||
|
(bom[2] == (char)0xBF);
|
||||||
|
|
||||||
|
// 不是BOM则将文件指针重置到开头，保证正常读取
|
||||||
|
if (!isBOM) {
|
||||||
|
file.seekg(0); // 重置文件指针到起始位置
|
||||||
|
}
|
||||||
|
// 是BOM则文件指针已在第4字节，无需处理
|
||||||
|
}
|
||||||
void _readMarkData(char* fileName, std::vector<SWD_charuco3DMark>& marks)
|
void _readMarkData(char* fileName, std::vector<SWD_charuco3DMark>& marks)
|
||||||
{
|
{
|
||||||
std::ifstream inputFile(fileName);
|
std::ifstream inputFile(fileName, std::ios::in | std::ios::binary);
|
||||||
|
skipUTF8BOM(inputFile);
|
||||||
std::string linedata;
|
std::string linedata;
|
||||||
while (getline(inputFile, linedata))
|
while (getline(inputFile, linedata))
|
||||||
{
|
{
|
||||||
@ -522,55 +541,86 @@ void _wirteVerifyResult(char* fileName,
|
|||||||
#define TEST_COMPUTE_CALIB_PARA 0
|
#define TEST_COMPUTE_CALIB_PARA 0
|
||||||
#define TEST_COMPUTE_CORNER 0
|
#define TEST_COMPUTE_CORNER 0
|
||||||
#define TEST_COMPUTE_POSE 1
|
#define TEST_COMPUTE_POSE 1
|
||||||
#define TEST_GROUP 2
|
#define TEST_GROUP 3
|
||||||
int main()
|
int main()
|
||||||
{
|
{
|
||||||
const char* dataPath[TEST_GROUP] = {
|
const char* dataPath[TEST_GROUP] = {
|
||||||
|
|
||||||
"F:/ShangGu/项目/冠钦_博清科技/组装/3D与Mark/21epic/", //0
|
"F:/ShangGu/项目/冠钦_博清科技/组装/3D与Mark/21epic/", //0
|
||||||
"F:/ShangGu/项目/冠钦_博清科技/组装/3D与Mark/20vizum/",
|
"F:/ShangGu/项目/冠钦_博清科技/组装/3D与Mark/20vizum/",
|
||||||
|
"F:/ShangGu/项目/冠钦_博清科技/组装/3D与Mark/现场_张奔_20260106/",
|
||||||
};
|
};
|
||||||
|
|
||||||
SVzNLRange fileIdx[TEST_GROUP] = {
|
SVzNLRange fileIdx[TEST_GROUP] = {
|
||||||
{2,33},
|
{2,33},{2,33}, {1,14}
|
||||||
};
|
};
|
||||||
|
|
||||||
#ifdef TEST_COMPUTE_POSE
|
#ifdef TEST_COMPUTE_POSE
|
||||||
double R_3D_mark[9] = {
|
double R_3D_mark[9] = {
|
||||||
9.7414217523834201e-01, -6.4917977205419487e-03, -2.2584259780711641e-01,
|
9.7432217968400503e-01, - 6.5983238824482409e-03,
|
||||||
6.4677677434932046e-03, 9.9997872563826395e-01, - 8.4631630596682995e-04,
|
- 2.2506166332308031e-01, 6.5293199817128209e-03,
|
||||||
2.2584328726426117e-01, - 6.3626506196937213e-04, 9.7416343842521791e-01
|
9.9997813155105764e-01, - 1.0509044828439812e-03,
|
||||||
|
2.2506367578173436e-01, - 4.4558006908867375e-04,
|
||||||
|
9.7434395533713669e-01
|
||||||
};
|
};
|
||||||
double T_3D_mark[3] = {
|
double T_3D_mark[3] = {
|
||||||
2.0118012741488724e+02, -3.4890651052684677e+02,-2.9090717029324466e+01
|
2.0100138599065690e+02, - 3.4778670146875356e+02,
|
||||||
|
- 2.8208030634996298e+01
|
||||||
};
|
};
|
||||||
double R_mark_3D[9] = {
|
double R_mark_3D[9] = {
|
||||||
9.7414217523834201e-01, 6.4677677434932046e-03, 2.2584328726426117e-01,
|
9.7432217968400503e-01, 6.5293199817128209e-03, 2.2506367578173436e-01,
|
||||||
-6.4917977205419487e-03, 9.9997872563826395e-01,-6.3626506196937213e-04,
|
- 6.5983238824482409e-03, 9.9997813155105764e-01,
|
||||||
-2.2584259780711641e-01, -8.4631630596682995e-04, 9.7416343842521791e-01
|
- 4.4558006908867375e-04, - 2.2506166332308031e-01,
|
||||||
|
- 1.0509044828439812e-03, 9.7434395533713669e-01
|
||||||
};
|
};
|
||||||
double T_mark_3D[3] = {
|
double T_mark_3D[3] = {
|
||||||
-1.8715145749760768e+02, 3.5018659904922669e+02, 7.3478870260969714e+01
|
-1.8722069479737289e+02, 3.4909279922237783e+02,
|
||||||
|
7.2356539798828123e+01
|
||||||
|
};
|
||||||
|
|
||||||
|
double R_ip21_ip20[9] = {
|
||||||
|
-9.0136463861749749e-01, 1.0494706583686632e-03,
|
||||||
|
- 4.3305968048444010e-01, 3.8989008937143142e-03,
|
||||||
|
- 9.9993686885077060e-01, - 1.0538353037260263e-02,
|
||||||
|
- 4.3304340062142632e-01, - 1.1187355552326124e-02,
|
||||||
|
9.0130364264989893e-01
|
||||||
|
};
|
||||||
|
double T_ip21_ip20[3] = {
|
||||||
|
7.4953277493063717e+02, 2.8014434657528108e+02, 1.7494775044638942e+02
|
||||||
|
};
|
||||||
|
double R_ip20_ip21[9] = {
|
||||||
|
-9.0136463861749749e-01, 3.8989008937143142e-03,
|
||||||
|
- 4.3304340062142632e-01, 1.0494706583686632e-03,
|
||||||
|
- 9.9993686885077060e-01, - 1.1187355552326124e-02,
|
||||||
|
- 4.3305968048444010e-01, - 1.0538353037260263e-02,
|
||||||
|
9.0130364264989893e-01
|
||||||
|
};
|
||||||
|
double T_ip20_ip21[3] = {
|
||||||
|
7.5027005254846563e+02, 2.8129725077327970e+02, 1.6986363929894415e+02
|
||||||
};
|
};
|
||||||
|
|
||||||
double R_3D_mark_20[9] = {
|
double R_3D_mark_20[9] = {
|
||||||
-9.7551111071544561e-01, 1.0398329224714965e-02,-2.1970422758802807e-01,
|
-9.7565715189866242e-01, 1.0207261472102205e-02,
|
||||||
- 8.3582333026743106e-03,- 9.9991291186786824e-01, -1.0213158961794689e-02,
|
- 2.1906376642939016e-01, - 8.0279186459975786e-03,
|
||||||
- 2.1979129374653508e-01, -8.1267108509691516e-03, 9.7551306693655782e-01,
|
- 9.9990905961091314e-01, - 1.0836283045053035e-02,
|
||||||
|
- 2.1915445345966281e-01, - 8.8138709577232511e-03,
|
||||||
|
9.7565036832235041e-01
|
||||||
};
|
};
|
||||||
double T_3D_mark_20[3] = {
|
double T_3D_mark_20[3] = {
|
||||||
5.7873032211966097e+02, 6.3216378913452741e+02, 6.5977504397803401e+01,
|
5.7984427943242531e+02, 6.3069724580763238e+02, 6.1343276694911815e+01
|
||||||
};
|
};
|
||||||
double R_mark_3D_20[9] = {
|
double R_mark_3D_20[9] = {
|
||||||
-9.7551111071544561e-01, -8.3582333026743106e-03, -2.1979129374653508e-01,
|
-9.7565715189866242e-01, - 8.0279186459975786e-03,
|
||||||
1.0398329224714965e-02, -9.9991291186786824e-01, -8.1267108509691516e-03,
|
- 2.1915445345966281e-01, 1.0207261472102205e-02,
|
||||||
-2.1970422758802807e-01, -1.0213158961794689e-02, 9.7551306693655782e-01
|
- 9.9990905961091314e-01, - 8.8138709577232511e-03,
|
||||||
|
- 2.1906376642939016e-01, - 1.0836283045053035e-02,
|
||||||
|
9.7565036832235041e-01
|
||||||
};
|
};
|
||||||
double T_mark_3D_20[3] = {
|
double T_mark_3D_20[3] = {
|
||||||
5.8434291282050799e+02, 6.2662708685011432e+02, 6.9243970007470736e+01
|
5.8423605667286893e+02, 6.2526193950634604e+02, 7.4007695164819168e+01
|
||||||
};
|
};
|
||||||
|
|
||||||
for (int grp = 0; grp <= 0; grp++)
|
for (int grp = 2; grp <= 2; grp++)
|
||||||
{
|
{
|
||||||
char _scan_file[256];
|
char _scan_file[256];
|
||||||
std::vector<std::vector< SVzNL3DPoint>> originMarksBuff;
|
std::vector<std::vector< SVzNL3DPoint>> originMarksBuff;
|
||||||
@ -582,18 +632,20 @@ int main()
|
|||||||
{
|
{
|
||||||
//fidx =4;
|
//fidx =4;
|
||||||
char _scan_file[256];
|
char _scan_file[256];
|
||||||
sprintf_s(_scan_file, "%sresult/LaserLine%d_corner_info.txt", dataPath[grp], fidx);
|
sprintf_s(_scan_file, "%sepic-cloud/result/LaserLine%d_corner_info.txt", dataPath[grp], fidx);
|
||||||
SSX_BQAssemblyInfo originPose_raw = _readPoseInfo(_scan_file);
|
SSX_BQAssemblyInfo pose_3D = _readPoseInfo(_scan_file);
|
||||||
SSX_BQAssemblyInfo originPose;
|
SSX_BQAssemblyInfo pose_3D_ip21;
|
||||||
originPose.O = _pointRT(originPose_raw.O, R_3D_mark, T_3D_mark);
|
pose_3D_ip21.O = _pointRT(pose_3D.O, R_3D_mark, T_3D_mark);
|
||||||
SSX_BQAssemblyInfo originPose_check;
|
SSX_BQAssemblyInfo originPose_ip21_ip20_check;
|
||||||
originPose_check.O = _pointRT(originPose.O, R_mark_3D, T_mark_3D);
|
originPose_ip21_ip20_check.O = _pointRT(pose_3D_ip21.O, R_ip21_ip20, T_ip21_ip20);
|
||||||
|
SSX_BQAssemblyInfo pose_3D_ip20;
|
||||||
|
pose_3D_ip20.O = _pointRT(pose_3D.O, R_3D_mark_20, T_3D_mark_20);
|
||||||
|
|
||||||
sprintf_s(_scan_file, "%sresult/marks_%d.txt", dataPath[grp], fidx);
|
sprintf_s(_scan_file, "%smark21/marks_%d.txt", dataPath[grp], fidx);
|
||||||
std::vector<SWD_charuco3DMark> marks_origin;
|
std::vector<SWD_charuco3DMark> marks_origin;
|
||||||
_readMarkData(_scan_file, marks_origin);
|
_readMarkData(_scan_file, marks_origin);
|
||||||
|
|
||||||
sprintf_s(_scan_file, "%sresult/marks_%d.txt", dataPath[grp+1], fidx);
|
sprintf_s(_scan_file, "%smark20/marks_%d.txt", dataPath[grp], fidx);
|
||||||
std::vector<SWD_charuco3DMark> marks_curr;
|
std::vector<SWD_charuco3DMark> marks_curr;
|
||||||
_readMarkData(_scan_file, marks_curr);
|
_readMarkData(_scan_file, marks_curr);
|
||||||
|
|
||||||
@ -628,7 +680,7 @@ int main()
|
|||||||
|
|
||||||
int errCode = 0;
|
int errCode = 0;
|
||||||
SSX_BQAssemblyInfo currPose = sx_BQ_computeAssemblyInfoFromMark(
|
SSX_BQAssemblyInfo currPose = sx_BQ_computeAssemblyInfoFromMark(
|
||||||
originPose, originMarkPos, currMarkPos, &errCode);
|
pose_3D_ip21, originMarkPos, currMarkPos, &errCode);
|
||||||
|
|
||||||
SSX_BQAssemblyInfo compute_testPose = sx_BQ_computeAssemblyInfoFromMark(
|
SSX_BQAssemblyInfo compute_testPose = sx_BQ_computeAssemblyInfoFromMark(
|
||||||
testPose_origin, originMarkPos, currMarkPos, &errCode);
|
testPose_origin, originMarkPos, currMarkPos, &errCode);
|
||||||
|
|||||||
@ -741,7 +741,7 @@ int main()
|
|||||||
const char* ver = wd_BQWorkpieceCornerVersion();
|
const char* ver = wd_BQWorkpieceCornerVersion();
|
||||||
printf("ver:%s\n", ver);
|
printf("ver:%s\n", ver);
|
||||||
|
|
||||||
for (int grp = 6; grp <= 7; grp++)
|
for (int grp = 7; grp <= 7; grp++)
|
||||||
{
|
{
|
||||||
SSG_planeCalibPara poseCalibPara;
|
SSG_planeCalibPara poseCalibPara;
|
||||||
//初始化成单位阵
|
//初始化成单位阵
|
||||||
@ -823,8 +823,8 @@ int main()
|
|||||||
growParam.minLTypeTreeLen = 100; //mm
|
growParam.minLTypeTreeLen = 100; //mm
|
||||||
growParam.minVTypeTreeLen = 100; //mm
|
growParam.minVTypeTreeLen = 100; //mm
|
||||||
SSX_BQworkpiecePara workpieceParam;
|
SSX_BQworkpiecePara workpieceParam;
|
||||||
if(grp == 7)
|
if (grp == 7)
|
||||||
workpieceParam.lineLen = 120.0; //直线段长度
|
workpieceParam.lineLen = 120; // 120.0; //直线段长度
|
||||||
else
|
else
|
||||||
workpieceParam.lineLen = 160.0; //直线段长度
|
workpieceParam.lineLen = 160.0; //直线段长度
|
||||||
int errCode = 0;
|
int errCode = 0;
|
||||||
|
|||||||
@ -183,6 +183,17 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "HC_channelSpaceMeasure_test
|
|||||||
{52A65444-8505-4FD5-9501-E2D297E2EB2C} = {52A65444-8505-4FD5-9501-E2D297E2EB2C}
|
{52A65444-8505-4FD5-9501-E2D297E2EB2C} = {52A65444-8505-4FD5-9501-E2D297E2EB2C}
|
||||||
EndProjectSection
|
EndProjectSection
|
||||||
EndProject
|
EndProject
|
||||||
|
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "rodAndBarDetection_test", "rodAndBarDetection_test\rodAndBarDetection_test.vcxproj", "{59F9508A-4295-4A7D-858C-7B4F0A7F8C82}"
|
||||||
|
ProjectSection(ProjectDependencies) = postProject
|
||||||
|
{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
|
||||||
|
{AEC22DC1-FBE0-4D9C-B090-D4821A9D846B} = {AEC22DC1-FBE0-4D9C-B090-D4821A9D846B}
|
||||||
|
EndProjectSection
|
||||||
|
EndProject
|
||||||
|
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "rodAndBarDetection", "rodAndBarDetection\rodAndBarDetection.vcxproj", "{AEC22DC1-FBE0-4D9C-B090-D4821A9D846B}"
|
||||||
|
ProjectSection(ProjectDependencies) = postProject
|
||||||
|
{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
|
||||||
|
EndProjectSection
|
||||||
|
EndProject
|
||||||
Global
|
Global
|
||||||
GlobalSection(SolutionConfigurationPlatforms) = preSolution
|
GlobalSection(SolutionConfigurationPlatforms) = preSolution
|
||||||
Debug|x64 = Debug|x64
|
Debug|x64 = Debug|x64
|
||||||
@ -471,6 +482,22 @@ Global
|
|||||||
{3EC47D19-2562-4303-82B9-6B1C93C5A37A}.Release|x64.Build.0 = Release|x64
|
{3EC47D19-2562-4303-82B9-6B1C93C5A37A}.Release|x64.Build.0 = Release|x64
|
||||||
{3EC47D19-2562-4303-82B9-6B1C93C5A37A}.Release|x86.ActiveCfg = Release|Win32
|
{3EC47D19-2562-4303-82B9-6B1C93C5A37A}.Release|x86.ActiveCfg = Release|Win32
|
||||||
{3EC47D19-2562-4303-82B9-6B1C93C5A37A}.Release|x86.Build.0 = Release|Win32
|
{3EC47D19-2562-4303-82B9-6B1C93C5A37A}.Release|x86.Build.0 = Release|Win32
|
||||||
|
{59F9508A-4295-4A7D-858C-7B4F0A7F8C82}.Debug|x64.ActiveCfg = Debug|x64
|
||||||
|
{59F9508A-4295-4A7D-858C-7B4F0A7F8C82}.Debug|x64.Build.0 = Debug|x64
|
||||||
|
{59F9508A-4295-4A7D-858C-7B4F0A7F8C82}.Debug|x86.ActiveCfg = Debug|Win32
|
||||||
|
{59F9508A-4295-4A7D-858C-7B4F0A7F8C82}.Debug|x86.Build.0 = Debug|Win32
|
||||||
|
{59F9508A-4295-4A7D-858C-7B4F0A7F8C82}.Release|x64.ActiveCfg = Release|x64
|
||||||
|
{59F9508A-4295-4A7D-858C-7B4F0A7F8C82}.Release|x64.Build.0 = Release|x64
|
||||||
|
{59F9508A-4295-4A7D-858C-7B4F0A7F8C82}.Release|x86.ActiveCfg = Release|Win32
|
||||||
|
{59F9508A-4295-4A7D-858C-7B4F0A7F8C82}.Release|x86.Build.0 = Release|Win32
|
||||||
|
{AEC22DC1-FBE0-4D9C-B090-D4821A9D846B}.Debug|x64.ActiveCfg = Debug|x64
|
||||||
|
{AEC22DC1-FBE0-4D9C-B090-D4821A9D846B}.Debug|x64.Build.0 = Debug|x64
|
||||||
|
{AEC22DC1-FBE0-4D9C-B090-D4821A9D846B}.Debug|x86.ActiveCfg = Debug|Win32
|
||||||
|
{AEC22DC1-FBE0-4D9C-B090-D4821A9D846B}.Debug|x86.Build.0 = Debug|Win32
|
||||||
|
{AEC22DC1-FBE0-4D9C-B090-D4821A9D846B}.Release|x64.ActiveCfg = Release|x64
|
||||||
|
{AEC22DC1-FBE0-4D9C-B090-D4821A9D846B}.Release|x64.Build.0 = Release|x64
|
||||||
|
{AEC22DC1-FBE0-4D9C-B090-D4821A9D846B}.Release|x86.ActiveCfg = Release|Win32
|
||||||
|
{AEC22DC1-FBE0-4D9C-B090-D4821A9D846B}.Release|x86.Build.0 = Release|Win32
|
||||||
EndGlobalSection
|
EndGlobalSection
|
||||||
GlobalSection(SolutionProperties) = preSolution
|
GlobalSection(SolutionProperties) = preSolution
|
||||||
HideSolutionNode = FALSE
|
HideSolutionNode = FALSE
|
||||||
|
|||||||
@ -81,6 +81,101 @@ void vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector<s
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void vzReadPlyTxtPointFromFile_XYZRGB_vector(const char* fileName, std::vector<std::vector< SVzNLPointXYZRGBA>>& scanData)
|
||||||
|
{
|
||||||
|
std::ifstream inputFile(fileName);
|
||||||
|
std::string linedata;
|
||||||
|
|
||||||
|
if (inputFile.is_open() == false)
|
||||||
|
return;
|
||||||
|
|
||||||
|
const double lineYGap = 50;
|
||||||
|
std::vector< SVzNLPointXYZRGBA> a_line;
|
||||||
|
|
||||||
|
double pre_y = FLT_MIN;
|
||||||
|
while (getline(inputFile, linedata))
|
||||||
|
{
|
||||||
|
SVzNLPointXYZRGBA a_pt;
|
||||||
|
memset(&a_pt, 0, sizeof(SVzNLPointXYZRGBA));
|
||||||
|
double _x, _y, _z;
|
||||||
|
sscanf_s(linedata.c_str(), "%lf %lf %lf", &_x, &_y, &_z);
|
||||||
|
a_pt.x = (float)_x;
|
||||||
|
a_pt.y = (float)_y;
|
||||||
|
a_pt.z = (float)_z;
|
||||||
|
if (a_pt.y < pre_y - lineYGap) //新的扫描行
|
||||||
|
{
|
||||||
|
if (a_line.size() > 0)
|
||||||
|
{
|
||||||
|
scanData.push_back(a_line);
|
||||||
|
a_line.clear();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
pre_y = a_pt.y;
|
||||||
|
a_line.push_back(a_pt);
|
||||||
|
}
|
||||||
|
if (a_line.size() > 0)
|
||||||
|
scanData.push_back(a_line);
|
||||||
|
|
||||||
|
inputFile.close();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
void _outputScanDataFile_XYZRGB(char* fileName, std::vector<std::vector< SVzNLPointXYZRGBA>>& 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++)
|
||||||
|
{
|
||||||
|
SVzNLPointXYZRGBA& pt3D = scanData[line][i];
|
||||||
|
float x = (float)pt3D.x;
|
||||||
|
float y = (float)pt3D.y;
|
||||||
|
float z = (float)pt3D.z;
|
||||||
|
sw << "{ " << x << "," << y << "," << z << " }-";
|
||||||
|
sw << "{0,0}-{0,0}" << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
sw.close();
|
||||||
|
}
|
||||||
|
|
||||||
|
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 _outputFillingPortInfo(char* fileName, SSG_6DOF centerPose)
|
void _outputFillingPortInfo(char* fileName, SSG_6DOF centerPose)
|
||||||
{
|
{
|
||||||
std::ofstream sw(fileName);
|
std::ofstream sw(fileName);
|
||||||
@ -208,11 +303,14 @@ void _outputRGBDScan_fillingPort_RGBD(
|
|||||||
sw.close();
|
sw.close();
|
||||||
}
|
}
|
||||||
|
|
||||||
#define TEST_GROUP 1
|
#define TEST_CONVERT_TO_GRID 1
|
||||||
|
#define TEST_COMPUTE_POSE 0
|
||||||
|
#define TEST_GROUP 2
|
||||||
int main()
|
int main()
|
||||||
{
|
{
|
||||||
const char* dataPath[TEST_GROUP] = {
|
const char* dataPath[TEST_GROUP] = {
|
||||||
"F:/ShangGu/项目/冠钦_LNG自动加气/加气口测试数据/", //0
|
"F:/ShangGu/项目/冠钦_LNG自动加气/加气口测试数据/", //0
|
||||||
|
"F:/ShangGu/项目/冠钦项目/圆环自动抓取/",
|
||||||
};
|
};
|
||||||
|
|
||||||
SVzNLRange fileIdx[TEST_GROUP] = {
|
SVzNLRange fileIdx[TEST_GROUP] = {
|
||||||
@ -222,6 +320,19 @@ int main()
|
|||||||
const char* ver = wd_gasFillingPortPositionVersion();
|
const char* ver = wd_gasFillingPortPositionVersion();
|
||||||
printf("ver:%s\n", ver);
|
printf("ver:%s\n", ver);
|
||||||
|
|
||||||
|
#if TEST_CONVERT_TO_GRID
|
||||||
|
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];
|
||||||
|
spr
|
||||||
|
LaserData_1
|
||||||
|
vzReadPlyTxtPointFromFile_XYZRGB_vector(const char* fileName, std::vector<std::vector< SVzNLPointXYZRGBA>>&scanData);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if TEST_COMPUTE_POSE
|
||||||
for (int grp = 0; grp < TEST_GROUP; grp++)
|
for (int grp = 0; grp < TEST_GROUP; grp++)
|
||||||
{
|
{
|
||||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||||
@ -277,6 +388,7 @@ int main()
|
|||||||
_outputFillingPortInfo(_scan_file, centerPose);
|
_outputFillingPortInfo(_scan_file, centerPose);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
|
// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
|
||||||
|
|||||||
170
rodAndBarDetection/rodAndBarDetection.vcxproj
Normal file
170
rodAndBarDetection/rodAndBarDetection.vcxproj
Normal file
@ -0,0 +1,170 @@
|
|||||||
|
<?xml version="1.0" encoding="utf-8"?>
|
||||||
|
<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||||
|
<ItemGroup Label="ProjectConfigurations">
|
||||||
|
<ProjectConfiguration Include="Debug|Win32">
|
||||||
|
<Configuration>Debug</Configuration>
|
||||||
|
<Platform>Win32</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
<ProjectConfiguration Include="Release|Win32">
|
||||||
|
<Configuration>Release</Configuration>
|
||||||
|
<Platform>Win32</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
<ProjectConfiguration Include="Debug|x64">
|
||||||
|
<Configuration>Debug</Configuration>
|
||||||
|
<Platform>x64</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
<ProjectConfiguration Include="Release|x64">
|
||||||
|
<Configuration>Release</Configuration>
|
||||||
|
<Platform>x64</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
</ItemGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<ClCompile Include="..\sourceCode\rodAndBarDetection.cpp" />
|
||||||
|
</ItemGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<ClInclude Include="..\sourceCode\rodAndBarDetection_Export.h" />
|
||||||
|
</ItemGroup>
|
||||||
|
<PropertyGroup Label="Globals">
|
||||||
|
<VCProjectVersion>16.0</VCProjectVersion>
|
||||||
|
<Keyword>Win32Proj</Keyword>
|
||||||
|
<ProjectGuid>{aec22dc1-fbe0-4d9c-b090-d4821a9d846b}</ProjectGuid>
|
||||||
|
<RootNamespace>rodAndBarDetection</RootNamespace>
|
||||||
|
<WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
|
||||||
|
</PropertyGroup>
|
||||||
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
|
||||||
|
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||||
|
<UseDebugLibraries>true</UseDebugLibraries>
|
||||||
|
<PlatformToolset>v142</PlatformToolset>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
|
||||||
|
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||||
|
<UseDebugLibraries>false</UseDebugLibraries>
|
||||||
|
<PlatformToolset>v142</PlatformToolset>
|
||||||
|
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
|
||||||
|
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||||
|
<UseDebugLibraries>true</UseDebugLibraries>
|
||||||
|
<PlatformToolset>v142</PlatformToolset>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
|
||||||
|
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||||
|
<UseDebugLibraries>false</UseDebugLibraries>
|
||||||
|
<PlatformToolset>v142</PlatformToolset>
|
||||||
|
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
</PropertyGroup>
|
||||||
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
|
||||||
|
<ImportGroup Label="ExtensionSettings">
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Label="Shared">
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<PropertyGroup Label="UserMacros" />
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||||
|
<LinkIncremental>true</LinkIncremental>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||||
|
<LinkIncremental>false</LinkIncremental>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||||
|
<LinkIncremental>true</LinkIncremental>
|
||||||
|
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
|
||||||
|
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||||
|
<LinkIncremental>false</LinkIncremental>
|
||||||
|
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
|
||||||
|
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||||
|
</PropertyGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||||
|
<ClCompile>
|
||||||
|
<WarningLevel>Level3</WarningLevel>
|
||||||
|
<SDLCheck>true</SDLCheck>
|
||||||
|
<PreprocessorDefinitions>WIN32;_DEBUG;RODANDBARDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||||
|
<ConformanceMode>true</ConformanceMode>
|
||||||
|
<PrecompiledHeader>Use</PrecompiledHeader>
|
||||||
|
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
|
||||||
|
</ClCompile>
|
||||||
|
<Link>
|
||||||
|
<SubSystem>Windows</SubSystem>
|
||||||
|
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||||
|
<EnableUAC>false</EnableUAC>
|
||||||
|
</Link>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||||
|
<ClCompile>
|
||||||
|
<WarningLevel>Level3</WarningLevel>
|
||||||
|
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||||
|
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||||
|
<SDLCheck>true</SDLCheck>
|
||||||
|
<PreprocessorDefinitions>WIN32;NDEBUG;RODANDBARDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||||
|
<ConformanceMode>true</ConformanceMode>
|
||||||
|
<PrecompiledHeader>Use</PrecompiledHeader>
|
||||||
|
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
|
||||||
|
</ClCompile>
|
||||||
|
<Link>
|
||||||
|
<SubSystem>Windows</SubSystem>
|
||||||
|
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||||
|
<OptimizeReferences>true</OptimizeReferences>
|
||||||
|
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||||
|
<EnableUAC>false</EnableUAC>
|
||||||
|
</Link>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||||
|
<ClCompile>
|
||||||
|
<WarningLevel>Level3</WarningLevel>
|
||||||
|
<SDLCheck>true</SDLCheck>
|
||||||
|
<PreprocessorDefinitions>_CRT_SECURE_NO_WARNINGS;_DEBUG;RODANDBARDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||||
|
<ConformanceMode>true</ConformanceMode>
|
||||||
|
<PrecompiledHeader>NotUsing</PrecompiledHeader>
|
||||||
|
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
|
||||||
|
</ClCompile>
|
||||||
|
<Link>
|
||||||
|
<SubSystem>Windows</SubSystem>
|
||||||
|
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||||
|
<EnableUAC>false</EnableUAC>
|
||||||
|
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||||
|
<AdditionalDependencies>opencv_world320d.lib;baseAlgorithm.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||||
|
</Link>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||||
|
<ClCompile>
|
||||||
|
<WarningLevel>Level3</WarningLevel>
|
||||||
|
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||||
|
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||||
|
<SDLCheck>true</SDLCheck>
|
||||||
|
<PreprocessorDefinitions>_CRT_SECURE_NO_WARNINGS;NDEBUG;RODANDBARDETECTION_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||||
|
<ConformanceMode>true</ConformanceMode>
|
||||||
|
<PrecompiledHeader>NotUsing</PrecompiledHeader>
|
||||||
|
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
|
||||||
|
</ClCompile>
|
||||||
|
<Link>
|
||||||
|
<SubSystem>Windows</SubSystem>
|
||||||
|
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||||
|
<OptimizeReferences>true</OptimizeReferences>
|
||||||
|
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||||
|
<EnableUAC>false</EnableUAC>
|
||||||
|
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||||
|
<AdditionalDependencies>opencv_world320.lib;baseAlgorithm.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||||
|
</Link>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
||||||
|
<ImportGroup Label="ExtensionTargets">
|
||||||
|
</ImportGroup>
|
||||||
|
</Project>
|
||||||
20
rodAndBarDetection_test/rodAndBarDetection_test.cpp
Normal file
20
rodAndBarDetection_test/rodAndBarDetection_test.cpp
Normal file
@ -0,0 +1,20 @@
|
|||||||
|
// rodAndBarDetection_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
|
||||||
|
//
|
||||||
|
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
int main()
|
||||||
|
{
|
||||||
|
std::cout << "Hello World!\n";
|
||||||
|
}
|
||||||
|
|
||||||
|
// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
|
||||||
|
// 调试程序: F5 或调试 >“开始调试”菜单
|
||||||
|
|
||||||
|
// 入门使用技巧:
|
||||||
|
// 1. 使用解决方案资源管理器窗口添加/管理文件
|
||||||
|
// 2. 使用团队资源管理器窗口连接到源代码管理
|
||||||
|
// 3. 使用输出窗口查看生成输出和其他消息
|
||||||
|
// 4. 使用错误列表窗口查看错误
|
||||||
|
// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目
|
||||||
|
// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件
|
||||||
157
rodAndBarDetection_test/rodAndBarDetection_test.vcxproj
Normal file
157
rodAndBarDetection_test/rodAndBarDetection_test.vcxproj
Normal file
@ -0,0 +1,157 @@
|
|||||||
|
<?xml version="1.0" encoding="utf-8"?>
|
||||||
|
<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||||
|
<ItemGroup Label="ProjectConfigurations">
|
||||||
|
<ProjectConfiguration Include="Debug|Win32">
|
||||||
|
<Configuration>Debug</Configuration>
|
||||||
|
<Platform>Win32</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
<ProjectConfiguration Include="Release|Win32">
|
||||||
|
<Configuration>Release</Configuration>
|
||||||
|
<Platform>Win32</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
<ProjectConfiguration Include="Debug|x64">
|
||||||
|
<Configuration>Debug</Configuration>
|
||||||
|
<Platform>x64</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
<ProjectConfiguration Include="Release|x64">
|
||||||
|
<Configuration>Release</Configuration>
|
||||||
|
<Platform>x64</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
</ItemGroup>
|
||||||
|
<PropertyGroup Label="Globals">
|
||||||
|
<VCProjectVersion>16.0</VCProjectVersion>
|
||||||
|
<Keyword>Win32Proj</Keyword>
|
||||||
|
<ProjectGuid>{59f9508a-4295-4a7d-858c-7b4f0a7f8c82}</ProjectGuid>
|
||||||
|
<RootNamespace>rodAndBarDetectiontest</RootNamespace>
|
||||||
|
<WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
|
||||||
|
</PropertyGroup>
|
||||||
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
|
||||||
|
<ConfigurationType>Application</ConfigurationType>
|
||||||
|
<UseDebugLibraries>true</UseDebugLibraries>
|
||||||
|
<PlatformToolset>v142</PlatformToolset>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
|
||||||
|
<ConfigurationType>Application</ConfigurationType>
|
||||||
|
<UseDebugLibraries>false</UseDebugLibraries>
|
||||||
|
<PlatformToolset>v142</PlatformToolset>
|
||||||
|
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
|
||||||
|
<ConfigurationType>Application</ConfigurationType>
|
||||||
|
<UseDebugLibraries>true</UseDebugLibraries>
|
||||||
|
<PlatformToolset>v142</PlatformToolset>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
|
||||||
|
<ConfigurationType>Application</ConfigurationType>
|
||||||
|
<UseDebugLibraries>false</UseDebugLibraries>
|
||||||
|
<PlatformToolset>v142</PlatformToolset>
|
||||||
|
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
</PropertyGroup>
|
||||||
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
|
||||||
|
<ImportGroup Label="ExtensionSettings">
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Label="Shared">
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<PropertyGroup Label="UserMacros" />
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||||
|
<LinkIncremental>true</LinkIncremental>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||||
|
<LinkIncremental>false</LinkIncremental>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||||
|
<LinkIncremental>true</LinkIncremental>
|
||||||
|
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
|
||||||
|
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||||
|
<LinkIncremental>false</LinkIncremental>
|
||||||
|
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
|
||||||
|
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||||
|
</PropertyGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||||
|
<ClCompile>
|
||||||
|
<WarningLevel>Level3</WarningLevel>
|
||||||
|
<SDLCheck>true</SDLCheck>
|
||||||
|
<PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||||
|
<ConformanceMode>true</ConformanceMode>
|
||||||
|
</ClCompile>
|
||||||
|
<Link>
|
||||||
|
<SubSystem>Console</SubSystem>
|
||||||
|
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||||
|
</Link>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||||
|
<ClCompile>
|
||||||
|
<WarningLevel>Level3</WarningLevel>
|
||||||
|
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||||
|
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||||
|
<SDLCheck>true</SDLCheck>
|
||||||
|
<PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||||
|
<ConformanceMode>true</ConformanceMode>
|
||||||
|
</ClCompile>
|
||||||
|
<Link>
|
||||||
|
<SubSystem>Console</SubSystem>
|
||||||
|
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||||
|
<OptimizeReferences>true</OptimizeReferences>
|
||||||
|
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||||
|
</Link>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||||
|
<ClCompile>
|
||||||
|
<WarningLevel>Level3</WarningLevel>
|
||||||
|
<SDLCheck>true</SDLCheck>
|
||||||
|
<PreprocessorDefinitions>_CRT_SECURE_NO_WARNINGS;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||||
|
<ConformanceMode>true</ConformanceMode>
|
||||||
|
<AdditionalIncludeDirectories>..\..\thirdParty\opencv320\build\include;</AdditionalIncludeDirectories>
|
||||||
|
</ClCompile>
|
||||||
|
<Link>
|
||||||
|
<SubSystem>Console</SubSystem>
|
||||||
|
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||||
|
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||||
|
<AdditionalDependencies>opencv_world320d.lib;rodAndBarDetection.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||||
|
</Link>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||||
|
<ClCompile>
|
||||||
|
<WarningLevel>Level3</WarningLevel>
|
||||||
|
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||||
|
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||||
|
<SDLCheck>true</SDLCheck>
|
||||||
|
<PreprocessorDefinitions>_CRT_SECURE_NO_WARNINGS;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||||
|
<ConformanceMode>true</ConformanceMode>
|
||||||
|
<AdditionalIncludeDirectories>..\..\thirdParty\opencv320\build\include;</AdditionalIncludeDirectories>
|
||||||
|
</ClCompile>
|
||||||
|
<Link>
|
||||||
|
<SubSystem>Console</SubSystem>
|
||||||
|
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||||
|
<OptimizeReferences>true</OptimizeReferences>
|
||||||
|
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||||
|
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||||
|
<AdditionalDependencies>opencv_world320.lib;rodAndBarDetection.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||||
|
</Link>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<ClCompile Include="rodAndBarDetection_test.cpp" />
|
||||||
|
</ItemGroup>
|
||||||
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
||||||
|
<ImportGroup Label="ExtensionTargets">
|
||||||
|
</ImportGroup>
|
||||||
|
</Project>
|
||||||
@ -410,6 +410,18 @@ SG_APISHARED_EXPORT void lineFitting_abc(
|
|||||||
double* _b,
|
double* _b,
|
||||||
double* _c);
|
double* _c);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 空间直线最小二乘拟合
|
||||||
|
* @param points 输入的三维点集（至少2个点，否则无意义）
|
||||||
|
* @param center 输出：拟合直线的质心（基准点P0）
|
||||||
|
* @param direction 输出：拟合直线的方向向量v（单位化）
|
||||||
|
* @return 拟合是否成功（点集有效返回true）
|
||||||
|
*/
|
||||||
|
SG_APISHARED_EXPORT bool fitLine3DLeastSquares(
|
||||||
|
const std::vector<SVzNL3DPoint>& points,
|
||||||
|
SVzNL3DPoint& center,
|
||||||
|
SVzNL3DPoint& direction);
|
||||||
|
|
||||||
//圆最小二乘拟合
|
//圆最小二乘拟合
|
||||||
SG_APISHARED_EXPORT double fitCircleByLeastSquare(
|
SG_APISHARED_EXPORT double fitCircleByLeastSquare(
|
||||||
const std::vector<SVzNL3DPoint>& pointArray,
|
const std::vector<SVzNL3DPoint>& pointArray,
|
||||||
@ -537,10 +549,15 @@ SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara_ROIs(
|
|||||||
//计算一个平面调平参数。
|
//计算一个平面调平参数。
|
||||||
//以数据输入中ROI以内的点进行平面拟合，计算调平参数
|
//以数据输入中ROI以内的点进行平面拟合，计算调平参数
|
||||||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||||||
SSG_planeCalibPara sg_getPlaneCalibPara2_ROI(
|
SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara2_ROI(
|
||||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||||
SVzNL3DRangeD roi);
|
SVzNL3DRangeD roi);
|
||||||
|
|
||||||
|
//根据两个向量计算旋转矩阵
|
||||||
|
SG_APISHARED_EXPORT SSG_planeCalibPara wd_computeRTMatrix(
|
||||||
|
SVzNL3DPoint& vector1,
|
||||||
|
SVzNL3DPoint& vector2);
|
||||||
|
|
||||||
// 从旋转矩阵计算欧拉角（Z-Y-X顺序）
|
// 从旋转矩阵计算欧拉角（Z-Y-X顺序）
|
||||||
SG_APISHARED_EXPORT SSG_EulerAngles rotationMatrixToEulerZYX(const double R[3][3]);
|
SG_APISHARED_EXPORT SSG_EulerAngles rotationMatrixToEulerZYX(const double R[3][3]);
|
||||||
// 从欧拉角计算旋转矩阵（Z-Y-X顺序）
|
// 从欧拉角计算旋转矩阵（Z-Y-X顺序）
|
||||||
|
|||||||
@ -2417,6 +2417,44 @@ SSG_planeCalibPara sg_HCameraVScan_getGroundCalibPara(
|
|||||||
return planePara;
|
return planePara;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
SSG_planeCalibPara wd_computeRTMatrix(SVzNL3DPoint& vector1, SVzNL3DPoint& vector2)
|
||||||
|
{
|
||||||
|
Vector3 a = Vector3(vector1.x, vector1.y, vector1.z);
|
||||||
|
Vector3 b = Vector3(vector2.x, vector2.y, vector2.z);
|
||||||
|
Quaternion quanPara = rotationBetweenVectors(a, b);
|
||||||
|
|
||||||
|
RotationMatrix rMatrix;
|
||||||
|
quaternionToMatrix(quanPara, rMatrix.data);
|
||||||
|
//计算反向旋转矩阵
|
||||||
|
Quaternion invQuanPara = rotationBetweenVectors(b, a);
|
||||||
|
RotationMatrix invMatrix;
|
||||||
|
quaternionToMatrix(invQuanPara, invMatrix.data);
|
||||||
|
|
||||||
|
SSG_planeCalibPara calibPara;
|
||||||
|
calibPara.planeCalib[0] = rMatrix.data[0][0];
|
||||||
|
calibPara.planeCalib[1] = rMatrix.data[0][1];
|
||||||
|
calibPara.planeCalib[2] = rMatrix.data[0][2];
|
||||||
|
calibPara.planeCalib[3] = rMatrix.data[1][0];
|
||||||
|
calibPara.planeCalib[4] = rMatrix.data[1][1];
|
||||||
|
calibPara.planeCalib[5] = rMatrix.data[1][2];
|
||||||
|
calibPara.planeCalib[6] = rMatrix.data[2][0];
|
||||||
|
calibPara.planeCalib[7] = rMatrix.data[2][1];
|
||||||
|
calibPara.planeCalib[8] = rMatrix.data[2][2];
|
||||||
|
|
||||||
|
calibPara.invRMatrix[0] = invMatrix.data[0][0];
|
||||||
|
calibPara.invRMatrix[1] = invMatrix.data[0][1];
|
||||||
|
calibPara.invRMatrix[2] = invMatrix.data[0][2];
|
||||||
|
calibPara.invRMatrix[3] = invMatrix.data[1][0];
|
||||||
|
calibPara.invRMatrix[4] = invMatrix.data[1][1];
|
||||||
|
calibPara.invRMatrix[5] = invMatrix.data[1][2];
|
||||||
|
calibPara.invRMatrix[6] = invMatrix.data[2][0];
|
||||||
|
calibPara.invRMatrix[7] = invMatrix.data[2][1];
|
||||||
|
calibPara.invRMatrix[8] = invMatrix.data[2][2];
|
||||||
|
|
||||||
|
calibPara.planeHeight = 0;
|
||||||
|
return calibPara;
|
||||||
|
}
|
||||||
|
|
||||||
SSG_planeCalibPara sg_getPlaneCalibPara2_ROI(
|
SSG_planeCalibPara sg_getPlaneCalibPara2_ROI(
|
||||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||||
SVzNL3DRangeD roi)
|
SVzNL3DRangeD roi)
|
||||||
@ -2796,7 +2834,7 @@ void lineDataRT(SVzNL3DLaserLine* a_line, const double* camPoseR, double groundH
|
|||||||
}
|
}
|
||||||
void lineDataRT_vector(std::vector< SVzNL3DPosition>& a_line, const double* camPoseR, double groundH)
|
void lineDataRT_vector(std::vector< SVzNL3DPosition>& a_line, const double* camPoseR, double groundH)
|
||||||
{
|
{
|
||||||
for (int i = 0; i < a_line.size(); i++)
|
for (int i = 0; i < (int)a_line.size(); i++)
|
||||||
{
|
{
|
||||||
SVzNL3DPoint a_pt = a_line[i].pt3D;
|
SVzNL3DPoint a_pt = a_line[i].pt3D;
|
||||||
if (a_pt.z < 1e-4)
|
if (a_pt.z < 1e-4)
|
||||||
@ -3034,3 +3072,59 @@ void pointRT(const cv::Mat& R, const cv::Mat& T,
|
|||||||
rtPt.z = result(2);
|
rtPt.z = result(2);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 空间直线最小二乘拟合
|
||||||
|
* @param points 输入的三维点集（至少2个点，否则无意义）
|
||||||
|
* @param center 输出：拟合直线的质心（基准点P0）
|
||||||
|
* @param direction 输出：拟合直线的方向向量v（单位化）
|
||||||
|
* @return 拟合是否成功（点集有效返回true）
|
||||||
|
*/
|
||||||
|
bool fitLine3DLeastSquares(const std::vector<SVzNL3DPoint>& points, SVzNL3DPoint& center, SVzNL3DPoint& direction)
|
||||||
|
{
|
||||||
|
// 检查点集有效性
|
||||||
|
if (points.size() < 2) {
|
||||||
|
std::cerr << "Error: 点集数量必须大于等于2！" << std::endl;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
int n = points.size();
|
||||||
|
Eigen::MatrixXd A(n, 3); // 点集矩阵：每行一个点的(x,y,z)
|
||||||
|
|
||||||
|
// 1. 计算质心（center）
|
||||||
|
double cx = 0.0, cy = 0.0, cz = 0.0;
|
||||||
|
for (const auto& p : points) {
|
||||||
|
cx += p.x;
|
||||||
|
cy += p.y;
|
||||||
|
cz += p.z;
|
||||||
|
A.row(points.size() - n) << p.x, p.y, p.z; // 填充点集矩阵
|
||||||
|
n--;
|
||||||
|
}
|
||||||
|
cx /= points.size();
|
||||||
|
cy /= points.size();
|
||||||
|
cz /= points.size();
|
||||||
|
center = { cx, cy, cz };
|
||||||
|
|
||||||
|
// 2. 构造去中心化的协方差矩阵（3x3）
|
||||||
|
// 关键修复：使用RowVector3d（行向量）做rowwise减法，匹配维度
|
||||||
|
Eigen::RowVector3d centroid_row(cx, cy, cz);
|
||||||
|
Eigen::MatrixXd centered = A.rowwise() - centroid_row; // 维度匹配，无报错
|
||||||
|
|
||||||
|
// 协方差矩阵计算（n-1为无偏估计，工程中也可直接用n）
|
||||||
|
Eigen::Matrix3d cov = centered.transpose() * centered / (points.size() - 1);
|
||||||
|
// 3. 特征值分解：求协方差矩阵的特征值和特征向量
|
||||||
|
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(cov);
|
||||||
|
if (eigensolver.info() != Eigen::Success) {
|
||||||
|
std::cerr << "Error: 特征值分解失败！" << std::endl;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 最大特征值对应的特征向量即为方向向量（Eigen默认按特征值升序排列，取最后一个）
|
||||||
|
Eigen::Vector3d dir = eigensolver.eigenvectors().col(2);
|
||||||
|
// 单位化方向向量（可选，但工程中通常标准化）
|
||||||
|
dir.normalize();
|
||||||
|
|
||||||
|
direction = { dir(0), dir(1), dir(2) };
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|||||||
@ -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, //定子的环宽度
|
double ringArcWidth, //»·¿í¶È
|
||||||
std::vector<SWD_segFeature>& line_ringArcs //»·
|
std::vector<SWD_segFeature>& line_ringArcs //»·
|
||||||
)
|
)
|
||||||
{
|
{
|
||||||
|
|||||||
299
sourceCode/rodAndBarDetection.cpp
Normal file
299
sourceCode/rodAndBarDetection.cpp
Normal file
@ -0,0 +1,299 @@
|
|||||||
|
#include <vector>
|
||||||
|
#include "SG_baseDataType.h"
|
||||||
|
#include "SG_baseAlgo_Export.h"
|
||||||
|
#include "rodAndBarDetection_Export.h"
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
#include <limits>
|
||||||
|
|
||||||
|
//version 1.0.0 : base version release to customer
|
||||||
|
std::string m_strVersion = "1.0.0";
|
||||||
|
const char* wd_rodAndBarDetectionVersion(void)
|
||||||
|
{
|
||||||
|
return m_strVersion.c_str();
|
||||||
|
}
|
||||||
|
|
||||||
|
SVzNL3DPoint getArcPeak(
|
||||||
|
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||||
|
SWD_segFeature & a_arcFeature)
|
||||||
|
{
|
||||||
|
SVzNL3DPoint arcPeak = scanLines[a_arcFeature.lineIdx][a_arcFeature.startPtIdx].pt3D;
|
||||||
|
for (int i = a_arcFeature.startPtIdx+1; i <= a_arcFeature.endPtIdx; i++)
|
||||||
|
{
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return arcPeak;
|
||||||
|
}
|
||||||
|
|
||||||
|
//投影，提取ROI内的数据
|
||||||
|
void xoyROIProjection(
|
||||||
|
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||||
|
const double* rtMatrix,
|
||||||
|
SSG_ROIRectD& roi_xoy,
|
||||||
|
std::vector<SVzNL3DPoint>& projectPoints
|
||||||
|
)
|
||||||
|
{
|
||||||
|
int lineNum = (int)scanLines.size();
|
||||||
|
for (int line = 0; line < lineNum; line++)
|
||||||
|
{
|
||||||
|
std::vector<SVzNL3DPosition>& a_line = scanLines[line];
|
||||||
|
int ptNum = (int)a_line.size();
|
||||||
|
for (int i = 0; i < (int)a_line.size(); i++)
|
||||||
|
{
|
||||||
|
SVzNL3DPoint a_pt = a_line[i].pt3D;
|
||||||
|
if (a_pt.z < 1e-4)
|
||||||
|
continue;
|
||||||
|
double x = a_pt.x * rtMatrix[0] + a_pt.y * rtMatrix[1] + a_pt.z * rtMatrix[2];
|
||||||
|
double y = a_pt.x * rtMatrix[3] + a_pt.y * rtMatrix[4] + a_pt.z * rtMatrix[5];
|
||||||
|
double z = a_pt.x * rtMatrix[6] + a_pt.y * rtMatrix[7] + a_pt.z * rtMatrix[8];
|
||||||
|
if ((x >= roi_xoy.left) && (x <= roi_xoy.right) &&
|
||||||
|
(y >= roi_xoy.top) && (y <= roi_xoy.bottom))
|
||||||
|
{
|
||||||
|
a_pt.x = x;
|
||||||
|
a_pt.y = y;
|
||||||
|
a_pt.z = z;
|
||||||
|
projectPoints.push_back(a_pt);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
SVzNLRangeD getZRange(std::vector<SVzNL3DPoint>& projectPoints)
|
||||||
|
{
|
||||||
|
int ptNum = (int)projectPoints.size();
|
||||||
|
SVzNLRangeD zRange;
|
||||||
|
zRange.min = DBL_MAX;
|
||||||
|
zRange.max = DBL_MIN;
|
||||||
|
for (int i = 0; i < ptNum; i++)
|
||||||
|
{
|
||||||
|
zRange.min = zRange.min > projectPoints[i].z ? projectPoints[i].z : zRange.min;
|
||||||
|
zRange.max = zRange.max < projectPoints[i].z ? projectPoints[i].z : zRange.max;
|
||||||
|
}
|
||||||
|
return zRange;
|
||||||
|
}
|
||||||
|
|
||||||
|
void zCutPointClouds(
|
||||||
|
std::vector<SVzNL3DPoint>& projectPoints,
|
||||||
|
SVzNLRangeD& zRange,
|
||||||
|
std::vector<SVzNL3DPoint>& cutLayerPoints)
|
||||||
|
{
|
||||||
|
int ptNum = (int)projectPoints.size();
|
||||||
|
for (int i = 0; i < ptNum; i++)
|
||||||
|
{
|
||||||
|
if ((projectPoints[i].z >= zRange.min) && (projectPoints[i].z <= zRange.max))
|
||||||
|
cutLayerPoints.push_back(projectPoints[i]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
SVzNL3DPoint getXoYCentroid(std::vector<SVzNL3DPoint>& points)
|
||||||
|
{
|
||||||
|
int ptNum = (int)points.size();
|
||||||
|
SVzNL3DPoint centroid = { 0.0, 0.0, 0.0 };
|
||||||
|
if (ptNum == 0)
|
||||||
|
return centroid;
|
||||||
|
for (int i = 0; i < ptNum; i++)
|
||||||
|
{
|
||||||
|
centroid.x += points[i].x;
|
||||||
|
centroid.y += points[i].y;
|
||||||
|
}
|
||||||
|
centroid.x = centroid.x / ptNum;
|
||||||
|
centroid.y = centroid.y / ptNum;
|
||||||
|
return centroid;
|
||||||
|
}
|
||||||
|
|
||||||
|
SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9])
|
||||||
|
{
|
||||||
|
SVzNL3DPoint _r_pt;
|
||||||
|
_r_pt.x = pt3D.x * matrix3d[0] + pt3D.y * matrix3d[1] + pt3D.z * matrix3d[2];
|
||||||
|
_r_pt.y = pt3D.x * matrix3d[3] + pt3D.y * matrix3d[4] + pt3D.z * matrix3d[5];
|
||||||
|
_r_pt.z = pt3D.x * matrix3d[6] + pt3D.y * matrix3d[7] + pt3D.z * matrix3d[8];
|
||||||
|
return _r_pt;
|
||||||
|
}
|
||||||
|
|
||||||
|
void sx_hexHeadScrewMeasure(
|
||||||
|
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||||
|
bool isHorizonScan, //true:激光线平行槽道；false:激光线垂直槽道
|
||||||
|
const SSG_cornerParam cornerPara,
|
||||||
|
const SSG_outlierFilterParam filterParam,
|
||||||
|
const SSG_treeGrowParam growParam,
|
||||||
|
double rodRidius,
|
||||||
|
std::vector<SSX_hexHeadScrewInfo>& screwInfo,
|
||||||
|
int* errCode)
|
||||||
|
{
|
||||||
|
*errCode = 0;
|
||||||
|
memset(&screwInfo, 0, sizeof(SSX_hexHeadScrewInfo));
|
||||||
|
int lineNum = (int)scanLines.size();
|
||||||
|
if (lineNum == 0)
|
||||||
|
{
|
||||||
|
*errCode = SG_ERR_3D_DATA_NULL;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector< std::vector<SVzNL3DPosition>> data_lines;
|
||||||
|
if (false == isHorizonScan)
|
||||||
|
{
|
||||||
|
data_lines.resize(lineNum);
|
||||||
|
for (int line = 0; line < lineNum; line++)
|
||||||
|
{
|
||||||
|
data_lines[line].insert(data_lines[line].end(), scanLines[line].begin(), scanLines[line].end());
|
||||||
|
for (int j = 0, j_max = (int)data_lines[line].size(); j < j_max; j++)
|
||||||
|
{
|
||||||
|
data_lines[line][j].nPointIdx = j;
|
||||||
|
scanLines[line][j].nPointIdx = 0; //转义复用
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
data_lines.resize(linePtNum);
|
||||||
|
for (int i = 0; i < linePtNum; i++)
|
||||||
|
data_lines[i].resize(lineNum);
|
||||||
|
for (int line = 0; line < lineNum; line++)
|
||||||
|
{
|
||||||
|
for (int j = 0; j < linePtNum; j++)
|
||||||
|
{
|
||||||
|
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||||||
|
data_lines[j][line] = scanLines[line][j];
|
||||||
|
data_lines[j][line].pt3D.x = scanLines[line][j].pt3D.y;
|
||||||
|
data_lines[j][line].pt3D.y = scanLines[line][j].pt3D.x;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
lineNum = linePtNum;
|
||||||
|
linePtNum = (int)data_lines[0].size();
|
||||||
|
for (int line = 0; line < lineNum; line++)
|
||||||
|
{
|
||||||
|
for (int j = 0, j_max = (int)data_lines[line].size(); j < j_max; j++)
|
||||||
|
data_lines[line][j].nPointIdx = j;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<std::vector<SWD_segFeature>> arcFeatures;
|
||||||
|
for (int line = 0; line < lineNum; line++)
|
||||||
|
{
|
||||||
|
if (line == 44)
|
||||||
|
int kkk = 1;
|
||||||
|
std::vector<SVzNL3DPosition>& lineData = data_lines[line];
|
||||||
|
|
||||||
|
//滤波，滤除异常点
|
||||||
|
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||||||
|
std::vector<SWD_segFeature> line_ringArcs;
|
||||||
|
int dataSize = (int)lineData.size();
|
||||||
|
//提取Arc特征
|
||||||
|
wd_getRingArcFeature(
|
||||||
|
lineData,
|
||||||
|
line, //当前扫描线序号
|
||||||
|
cornerPara,
|
||||||
|
rodRidius / 2, //环宽度
|
||||||
|
line_ringArcs //环
|
||||||
|
);
|
||||||
|
arcFeatures.push_back(line_ringArcs);
|
||||||
|
}
|
||||||
|
//特征生长
|
||||||
|
std::vector<SWD_segFeatureTree> growTrees;
|
||||||
|
wd_getSegFeatureGrowingTrees(
|
||||||
|
arcFeatures,
|
||||||
|
growTrees,
|
||||||
|
growParam);
|
||||||
|
|
||||||
|
if (growTrees.size() == 0)
|
||||||
|
{
|
||||||
|
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
int objNum = (int)growTrees.size();
|
||||||
|
for (int i = 0; i < objNum; i++)
|
||||||
|
{
|
||||||
|
//空间直线拟合
|
||||||
|
std::vector<SVzNL3DPoint> fitPoints;
|
||||||
|
int nodeSize = (int)growTrees[i].treeNodes.size();
|
||||||
|
for (int j = 0; j < nodeSize; j++)
|
||||||
|
{
|
||||||
|
SVzNL3DPoint a_pt = getArcPeak(data_lines, growTrees[i].treeNodes[j]);
|
||||||
|
fitPoints.push_back(a_pt);
|
||||||
|
}
|
||||||
|
if (fitPoints.size() < 12)
|
||||||
|
continue;
|
||||||
|
//去除头尾各5个点，防止在端部和根部扫描时的数据有干扰
|
||||||
|
fitPoints.erase(fitPoints.begin(), fitPoints.begin() + 5);
|
||||||
|
fitPoints.erase(fitPoints.end() - 5, fitPoints.end());
|
||||||
|
//拟合
|
||||||
|
SVzNL3DPoint P0_center, P1_dir;
|
||||||
|
bool result = fitLine3DLeastSquares(fitPoints, P0_center, P1_dir);
|
||||||
|
if (false == result)
|
||||||
|
continue;
|
||||||
|
//投影
|
||||||
|
//计算旋转向量
|
||||||
|
SVzNL3DPoint vector1 = P1_dir;
|
||||||
|
SVzNL3DPoint vector2 = { 0, 0, -1.0 };
|
||||||
|
SSG_planeCalibPara rotatePara = wd_computeRTMatrix( vector1, vector2);
|
||||||
|
//
|
||||||
|
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范围
|
||||||
|
std::vector< SVzNL3DPoint> roiProjectionData;
|
||||||
|
xoyROIProjection(scanLines, rotatePara.planeCalib, roi_xoy, roiProjectionData);
|
||||||
|
//取端面
|
||||||
|
SVzNLRangeD zRange = getZRange(roiProjectionData);
|
||||||
|
SVzNLRangeD cutZRange;
|
||||||
|
cutZRange.min = zRange.min;
|
||||||
|
cutZRange.max = zRange.min + 10.0; //取10mm的端面
|
||||||
|
std::vector<SVzNL3DPoint> surfacePoints;
|
||||||
|
zCutPointClouds(roiProjectionData, cutZRange, surfacePoints);
|
||||||
|
//计算中心点
|
||||||
|
SVzNL3DPoint projectionCenter = getXoYCentroid(surfacePoints);
|
||||||
|
projectionCenter.z = zRange.min;
|
||||||
|
//旋转回原坐标系
|
||||||
|
SVzNL3DPoint surfaceCenter = _ptRotate(projectionCenter, rotatePara.invRMatrix);
|
||||||
|
//生成Rod信息
|
||||||
|
SSX_hexHeadScrewInfo a_rod;
|
||||||
|
a_rod.center = surfaceCenter;
|
||||||
|
a_rod.axialDir = P1_dir;
|
||||||
|
a_rod.rotateAngle = 0;
|
||||||
|
screwInfo.push_back(a_rod);
|
||||||
|
}
|
||||||
|
if (true == isHorizonScan)
|
||||||
|
{
|
||||||
|
int objNum = (int)screwInfo.size();
|
||||||
|
for (int i = 0; i < objNum; i++)
|
||||||
|
{
|
||||||
|
double tmp = screwInfo[i].center.x;
|
||||||
|
screwInfo[i].center.x = screwInfo[i].center.y;
|
||||||
|
screwInfo[i].center.y = tmp;
|
||||||
|
tmp = screwInfo[i].axialDir.x;
|
||||||
|
screwInfo[i].axialDir.x = screwInfo[i].axialDir.y;
|
||||||
|
screwInfo[i].axialDir.y = tmp;
|
||||||
|
//screwInfo[i].rotateAngle += 90;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
27
sourceCode/rodAndBarDetection_Export.h
Normal file
27
sourceCode/rodAndBarDetection_Export.h
Normal file
@ -0,0 +1,27 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "SG_algo_Export.h"
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#define _OUTPUT_DEBUG_DATA 1
|
||||||
|
|
||||||
|
typedef struct
|
||||||
|
{
|
||||||
|
SVzNL3DPoint center; //螺杆端部中心点
|
||||||
|
SVzNL3DPoint axialDir; //轴向向量
|
||||||
|
double rotateAngle; //-30 - 30度
|
||||||
|
}SSX_hexHeadScrewInfo; //六角头螺杆
|
||||||
|
|
||||||
|
//读版本号
|
||||||
|
SG_APISHARED_EXPORT const char* wd_rodAndBarDetectionVersion(void);
|
||||||
|
|
||||||
|
//六角头螺杆端部中心点和轴向测量
|
||||||
|
SG_APISHARED_EXPORT void sx_hexHeadScrewMeasure(
|
||||||
|
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||||
|
bool isHorizonScan, //true:激光线平行槽道；false:激光线垂直槽道
|
||||||
|
const SSG_cornerParam cornerPara,
|
||||||
|
const SSG_outlierFilterParam filterParam,
|
||||||
|
const SSG_treeGrowParam growParam,
|
||||||
|
double rodRidius,
|
||||||
|
std::vector<SSX_hexHeadScrewInfo>& screwInfo,
|
||||||
|
int* errCode);
|
||||||
@ -2637,7 +2637,7 @@ void _outputScanDataFile_removeZeros(char* fileName, SVzNL3DLaserLine * scanData
|
|||||||
|
|
||||||
#define TEST_CONVERT_TO_GRID 0
|
#define TEST_CONVERT_TO_GRID 0
|
||||||
#define TEST_COMPUTE_WHEEL_ARCH 1
|
#define TEST_COMPUTE_WHEEL_ARCH 1
|
||||||
#define TEST_COMPUTE_CALIB_PARA 0
|
#define TEST_COMPUTE_CALIB_PARA 1
|
||||||
|
|
||||||
#define TEST_GROUP 1
|
#define TEST_GROUP 1
|
||||||
int main()
|
int main()
|
||||||
@ -2683,7 +2683,7 @@ int main()
|
|||||||
|
|
||||||
#if TEST_COMPUTE_CALIB_PARA
|
#if TEST_COMPUTE_CALIB_PARA
|
||||||
char _calib_datafile[256];
|
char _calib_datafile[256];
|
||||||
sprintf_s(_calib_datafile, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据2/ground_LaserData.txt");
|
sprintf_s(_calib_datafile, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据/ground_LaserData.txt");
|
||||||
|
|
||||||
std::vector< std::vector<SVzNL3DPosition>> scanLines;
|
std::vector< std::vector<SVzNL3DPosition>> scanLines;
|
||||||
vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanLines);
|
vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanLines);
|
||||||
@ -2704,10 +2704,10 @@ int main()
|
|||||||
#endif
|
#endif
|
||||||
//
|
//
|
||||||
char calibFile[250];
|
char calibFile[250];
|
||||||
sprintf_s(calibFile, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据2/ground_calib_para.txt");
|
sprintf_s(calibFile, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据/ground_calib_para.txt");
|
||||||
_outputCalibPara(calibFile, calibPara);
|
_outputCalibPara(calibFile, calibPara);
|
||||||
char _out_file[256];
|
char _out_file[256];
|
||||||
sprintf_s(_out_file, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据2/LaserLine_grund_calib.txt");
|
sprintf_s(_out_file, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据/LaserLine_grund_calib.txt");
|
||||||
_outputScanDataFile_self(_out_file, scanLines, 0, 0, 0);
|
_outputScanDataFile_self(_out_file, scanLines, 0, 0, 0);
|
||||||
printf("%s: calib done!\n", _calib_datafile);
|
printf("%s: calib done!\n", _calib_datafile);
|
||||||
}
|
}
|
||||||
@ -2716,11 +2716,11 @@ int main()
|
|||||||
|
|
||||||
#if TEST_COMPUTE_WHEEL_ARCH
|
#if TEST_COMPUTE_WHEEL_ARCH
|
||||||
const char* dataPath[TEST_GROUP] = {
|
const char* dataPath[TEST_GROUP] = {
|
||||||
"F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据2/", //0
|
"F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据/", //0
|
||||||
};
|
};
|
||||||
|
|
||||||
SVzNLRange fileIdx[TEST_GROUP] = {
|
SVzNLRange fileIdx[TEST_GROUP] = {
|
||||||
{1,1},
|
{1,2},
|
||||||
};
|
};
|
||||||
|
|
||||||
SSG_planeCalibPara poseCalibPara;
|
SSG_planeCalibPara poseCalibPara;
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user