From 47e3a04bf07414f54df25a2a3d55533f504b0cad Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Mon, 5 Jan 2026 01:19:04 +0800 Subject: [PATCH] =?UTF-8?q?HC=5FchanelSpaceMeasure=20version=201.0.0=20?= =?UTF-8?q?=E6=A7=BD=E9=81=93=E9=97=B4=E8=B7=9D=E6=A3=80=E6=B5=8B=E5=88=9D?= =?UTF-8?q?=E5=A7=8B=E7=89=88=E6=9C=AC=EF=BC=8C=20=E5=8C=85=E5=90=AB?= =?UTF-8?q?=E4=BA=86=E5=8D=9A=E6=B8=85=E5=B7=A5=E4=BB=B6=E7=BB=84=E8=A3=85?= =?UTF-8?q?=E7=9A=84=E6=9C=80=E6=96=B0=E4=BF=AE=E6=94=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../BQ_assemblyPosition_test.cpp | 791 ++++++++++- .../BQ_assemblyPosition_test.vcxproj | 14 +- .../BinocularMarkCam_test.cpp | 29 +- .../HC_chanelSpaceMeasure_test.cpp | 361 +++++ .../HC_chanelSpaceMeasure_test.vcxproj | 158 +++ .../HC_channelSpaceMeausre.vcxproj | 173 +++ SG_Algorithm.sln | 11 + .../motorStatorPosition_test.cpp | 470 +++---- sourceCode/BQ_assemblyPosition.cpp | 286 +++- sourceCode/BQ_assemblyPosition_Export.h | 19 +- sourceCode/SG_baseAlgo_Export.h | 34 + sourceCode/SG_baseDataType.h | 13 + sourceCode/SG_baseFunc.cpp | 159 ++- sourceCode/SG_featureGrow.cpp | 106 ++ sourceCode/SG_lineFeature.cpp | 1188 +++++++++++++++-- sourceCode/channelSpaceMeasure.cpp | 371 +++++ sourceCode/channelSpaceMeasure_Export.h | 32 + sourceCode/gasFillingPortPosition.cpp | 2 +- sourceCode/motorStatorPosition.cpp | 197 ++- sourceCode/motorStatorPosition_Export.h | 13 +- .../wheelArchHeigthMeasure_test.cpp | 11 +- 21 files changed, 4008 insertions(+), 430 deletions(-) create mode 100644 HC_chanelSpaceMeasure_test/HC_chanelSpaceMeasure_test.cpp create mode 100644 HC_chanelSpaceMeasure_test/HC_chanelSpaceMeasure_test.vcxproj create mode 100644 HC_channelSpaceMeausre/HC_channelSpaceMeausre.vcxproj create mode 100644 sourceCode/channelSpaceMeasure.cpp create mode 100644 sourceCode/channelSpaceMeasure_Export.h diff --git a/BQ_assemblyPosition_test/BQ_assemblyPosition_test.cpp b/BQ_assemblyPosition_test/BQ_assemblyPosition_test.cpp index b5a7a67..b107625 100644 --- a/BQ_assemblyPosition_test/BQ_assemblyPosition_test.cpp +++ b/BQ_assemblyPosition_test/BQ_assemblyPosition_test.cpp @@ -1,11 +1,798 @@ -// BQ_assemblyPosition_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。 +// BQ_workpieceCornerExtract_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。 // #include +#include +#include +#include +#include +#include "direct.h" +#include +#include "BQ_assemblyPosition_Export.h" +#include +#include +#include +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 vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector>& 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; + //迁移相机的XY次序和伟景相机正好相反 + sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &Y, &X, &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 wdSavePlyTxt(const char* fileName, std::vector> scanLines) +{ + std::ofstream sw(fileName); + int lineNum = scanLines.size(); + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = scanLines[line].size(); + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPoint* pt3D = &scanLines[line][i].pt3D; + if (pt3D->z < 1e-4) + continue; + double x = (double)pt3D->x; + double y = (double)pt3D->y; + double z = (double)pt3D->z; + sw << x << "," << y << "," << z << std::endl; + } + } + sw.close(); +} + +void _convertToGridData_XYZ_vector(std::vector>& scanData, double _F, std::vector>& scanData_grid) +{ + int min_y = 100000000; + int max_y = -10000000; + int lineNum = scanData.size(); + for (int line = 0; line < lineNum; line++) + { + std::vector< SVzNL3DPosition>& a_line = scanData[line]; + int nPointCnt = a_line.size(); + for (int i = 0; i < nPointCnt; i++) + { + SVzNL3DPosition* a_pt = &scanData[line][i]; + if (a_pt->pt3D.z > 1e-4) + { + double v = _F * a_pt->pt3D.y / a_pt->pt3D.z + 2000; + a_pt->nPointIdx = (int)(v + 0.5); + max_y = max_y < (int)a_pt->nPointIdx ? (int)a_pt->nPointIdx : max_y; + min_y = min_y > (int)a_pt->nPointIdx ? (int)a_pt->nPointIdx : min_y; + } + } + } + if (min_y == 100000000) + return; + + int pt_counter = max_y - min_y + 1; + for (int line = 0; line < lineNum; line++) + { + std::vector< SVzNL3DPosition> gridData; + gridData.resize(pt_counter); + for (int i = 0; i < pt_counter; i++) + gridData[i] = { 0,{ 0.0, 0.0, 0.0} }; + + std::vector< SVzNL3DPosition>& a_line = scanData[line]; + int nPointCnt = a_line.size(); + for (int i = 0; i < nPointCnt; i++) + { + SVzNL3DPosition a_pt = a_line[i]; + if (a_pt.pt3D.z > 1e-4) + { + int pt_id = a_pt.nPointIdx - min_y; + gridData[pt_id] = a_pt; + } + } + scanData_grid.push_back(gridData); + } + return; +} + +void _outputScanDataFile_XYZ_vector(char* fileName, std::vector>& scanData) +{ + std::ofstream sw(fileName); + int lineNum = scanData.size(); + sw << "LineNum:" << lineNum << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed: 0" << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp: 0_0" << std::endl; + + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = 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; + char str[250]; + sprintf_s(str, "{ %f, %f, %f } - { 0, 0 } - { 0, 0 }", x, y, z); + + sw << str << std::endl; + } + } + sw.close(); +} + +void _getRoiData_XYZ_vector( + std::vector>& scanData, + std::vector>& roiData, + SVzNL3DRangeD roi) +{ + int lineNum = scanData.size(); + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = scanData[line].size(); + std::vector< SVzNL3DPosition> linePts; + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPosition pt3D = scanData[line][i]; + if ((pt3D.pt3D.z >= roi.zRange.min) && + (pt3D.pt3D.z <= roi.zRange.max) && + (pt3D.pt3D.y >= roi.yRange.min) && + (pt3D.pt3D.y <= roi.yRange.max)) + { + linePts.push_back(pt3D); + } + } + roiData.push_back(linePts); + } +} + +void _outputCalibPara(char* fileName, SSG_planeCalibPara calibPara) +{ + std::ofstream sw(fileName); + char dataStr[250]; + //调平矩阵 + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[0], calibPara.planeCalib[1], calibPara.planeCalib[2]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[3], calibPara.planeCalib[4], calibPara.planeCalib[5]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[6], calibPara.planeCalib[7], calibPara.planeCalib[8]); + sw << dataStr << std::endl; + //地面高度 + sprintf_s(dataStr, 250, "%g", calibPara.planeHeight); + sw << dataStr << std::endl; + //反向旋转矩阵 + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[0], calibPara.invRMatrix[1], calibPara.invRMatrix[2]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[3], calibPara.invRMatrix[4], calibPara.invRMatrix[5]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[6], calibPara.invRMatrix[7], calibPara.invRMatrix[8]); + sw << dataStr << std::endl; + + sw.close(); +} + +void _outputPoseInfo(char* fileName, SSX_BQAssemblyInfo assemblyPose) +{ + std::ofstream sw(fileName); + char dataStr[250]; + sprintf_s(dataStr, 250, " O: (%g, %g, %g)", assemblyPose.O.x, assemblyPose.O.y, assemblyPose.O.z); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, " X: (%g, %g, %g)", assemblyPose.X.x, assemblyPose.X.y, assemblyPose.X.z); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, " Y: (%g, %g, %g)", assemblyPose.Y.x, assemblyPose.Y.y, assemblyPose.Y.z); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, " Z: (%g, %g, %g)", assemblyPose.Z.x, assemblyPose.Z.y, assemblyPose.Z.z); + sw << dataStr << std::endl; + sw.close(); +} + +SSX_BQAssemblyInfo _readPoseInfo(char* fileName) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + SSX_BQAssemblyInfo pose; + memset(&pose, 0, sizeof(SSX_BQAssemblyInfo)); + if (inputFile.is_open() == false) + return pose; + + getline(inputFile, linedata); + sscanf_s(linedata.c_str(), " O: (%lf, %lf, %lf)", &pose.O.x, &pose.O.y, &pose.O.z); + getline(inputFile, linedata); + sscanf_s(linedata.c_str(), " X: (%lf, %lf, %lf)", &pose.X.x, &pose.X.y, &pose.X.z); + getline(inputFile, linedata); + sscanf_s(linedata.c_str(), " Y: (%lf, %lf, %lf)", &pose.Y.x, &pose.Y.y, &pose.Y.z); + getline(inputFile, linedata); + sscanf_s(linedata.c_str(), " Z: (%lf, %lf, %lf)", &pose.Z.x, &pose.Z.y, &pose.Z.z); + + inputFile.close(); + return pose; +} + +void _readMarkData(char* fileName, std::vector& marks) +{ + std::ifstream inputFile(fileName); + std::string linedata; + while (getline(inputFile, linedata)) + { + SWD_charuco3DMark a_mark; + float x, y, z; + sscanf_s(linedata.c_str(), "MarkID_%d: %f, %f, %f", &a_mark.markID, &x, &y, &z); + a_mark.mark3D = { x, y, z }; + marks.push_back(a_mark); + } + inputFile.close(); + return; +} + +void _outputScanDataFile_vector(char* fileName, std::vector>& scanLines, bool removeZeros, int* headNullLines) +{ + std::ofstream sw(fileName); + int lineNum = (int)scanLines.size(); + if (lineNum == 0) + return; + + sw << "LineNum:" << lineNum << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed: 0" << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp: 0_0" << std::endl; + + int lineIdx = 0; + int null_lines = 0; + bool counterNull = true; + for (int line = 0; line < lineNum; line++) + { + int linePtNum = (int)scanLines[line].size(); + if (linePtNum == 0) + continue; + + if (true == removeZeros) + { + int vldPtNum = 0; + for (int i = 0; i < linePtNum; i++) + { + if (scanLines[line][i].pt3D.z > 1e-4) + vldPtNum++; + } + linePtNum = vldPtNum; + } + sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl; + lineIdx++; + bool isNull = true; + for (int i = 0; i < linePtNum; i++) + { + SVzNL3DPoint* pt3D = &scanLines[line][i].pt3D; + if ((pt3D->z > 1e-4) && (isNull == true)) + isNull = false; + if ((true == removeZeros) && (pt3D->z < 1e-4)) + continue; + 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; + } + if (true == counterNull) + { + if (true == isNull) + null_lines++; + else + counterNull = false; + } + } + *headNullLines = null_lines; + sw.close(); +} + +SSG_planeCalibPara _readCalibPara(char* fileName) +{ + //设置初始结果 + double initCalib[9] = { + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 }; + SSG_planeCalibPara planePara; + for (int i = 0; i < 9; i++) + planePara.planeCalib[i] = initCalib[i]; + planePara.planeHeight = -1.0; + for (int i = 0; i < 9; i++) + planePara.invRMatrix[i] = initCalib[i]; + + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return planePara; + + //调平矩阵 + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[0], &planePara.planeCalib[1], &planePara.planeCalib[2]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[3], &planePara.planeCalib[4], &planePara.planeCalib[5]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[6], &planePara.planeCalib[7], &planePara.planeCalib[8]); + //地面高度 + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf", &planePara.planeHeight); + //反向旋转矩阵 + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[0], &planePara.invRMatrix[1], &planePara.invRMatrix[2]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[3], &planePara.invRMatrix[4], &planePara.invRMatrix[5]); + std::getline(inputFile, linedata); + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[6], &planePara.invRMatrix[7], &planePara.invRMatrix[8]); + + inputFile.close(); + return planePara; +} + +void _outputRGBDScanLapWeld_RGBD( + char* fileName, + std::vector>& scanLines, + SSX_BQAssemblyInfo assemblyPose) +{ + int lineNum = (int)scanLines.size(); + std::ofstream sw(fileName); + int realLines = lineNum; + realLines++; + 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 > 0) + int kkk = 1; + int featureType_v = pt3D->nPointIdx & 0xff; + int featureType_h = featureType_v >> 4; + featureType_v &= 0x0f; + + if (pt3D->nPointIdx == 1) + { + rgb = { 255, 97, 0 }; + size = 5; + } + else + { + 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; + } + } + + int linePtNum = 1; + sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl; + lineNum++; + + rgb = { 255, 0, 0 }; + size = 15; + float x = (float)assemblyPose.O.x; + float y = (float)assemblyPose.O.y; + float z = (float)assemblyPose.O.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + sw.close(); +} + +SVzNL3DPoint _pointRT(SVzNL3DPoint& origin, const double* R, const double* T) +{ + SVzNL3DPoint result; + result.x = origin.x * R[0] + origin.y * R[1] + origin.z * R[2]; + result.y = origin.x * R[3] + origin.y * R[4] + origin.z * R[5]; + result.z = origin.x * R[6] + origin.y * R[7] + origin.z * R[8]; + result.x += T[0]; + result.y += T[1]; + result.z += T[2]; + return result; +} + + +void _wirteVerifyResult(char* fileName, + std::vector>& originMarksBuff, + std::vector>& currMarksBuff, + std::vector< SSX_BQAssemblyInfo>& oiginPoseBuff, + std::vector< SSX_BQAssemblyInfo>& computePoseBuff, + std::vector< SSX_BQAssemblyInfo>& verifyPoseBuff) +{ + std::ofstream sw(fileName); + char dataStr[250]; + + for (int i = 0; i < (int)originMarksBuff.size(); i++) + { + int markSize = (int)originMarksBuff[i].size(); + sprintf_s(dataStr, 250, "origin: "); + for (int j = 0; j < markSize; j++) + { + sprintf_s(dataStr, 250, "{ %g, %g, %g },", originMarksBuff[i][j].x, originMarksBuff[i][j].y, originMarksBuff[i][j].z); + sw << dataStr; + } + sprintf_s(dataStr, 250, " operate: "); + for (int j = 0; j < markSize; j++) + { + sprintf_s(dataStr, 250, "{ %g, %g, %g },", currMarksBuff[i][j].x, currMarksBuff[i][j].y, currMarksBuff[i][j].z); + sw << dataStr; + } + sprintf_s(dataStr, 250, " oriPose: { %g, %g, %g },", oiginPoseBuff[i].O.x, oiginPoseBuff[i].O.y, oiginPoseBuff[i].O.z); + sw << dataStr; + sprintf_s(dataStr, 250, " verifyPose: { %g, %g, %g },", verifyPoseBuff[i].O.x, verifyPoseBuff[i].O.y, verifyPoseBuff[i].O.z); + sw << dataStr; + sprintf_s(dataStr, 250, " computePose: { %g, %g, %g },", computePoseBuff[i].O.x, computePoseBuff[i].O.y, computePoseBuff[i].O.z); + sw << dataStr << std::endl; + } + sw.close(); + return; +} + +#define CONVERT_TO_GRID 0 +#define TEST_COMPUTE_CALIB_PARA 0 +#define TEST_COMPUTE_CORNER 0 +#define TEST_COMPUTE_POSE 1 +#define TEST_GROUP 2 int main() { - std::cout << "Hello World!\n"; + const char* dataPath[TEST_GROUP] = { + + "F:/ShangGu/项目/冠钦_博清科技/组装/3D与Mark/21epic/", //0 + "F:/ShangGu/项目/冠钦_博清科技/组装/3D与Mark/20vizum/", + }; + + SVzNLRange fileIdx[TEST_GROUP] = { + {2,33}, + }; + +#ifdef TEST_COMPUTE_POSE + double R_3D_mark[9] = { + 9.7414217523834201e-01, -6.4917977205419487e-03, -2.2584259780711641e-01, + 6.4677677434932046e-03, 9.9997872563826395e-01, - 8.4631630596682995e-04, + 2.2584328726426117e-01, - 6.3626506196937213e-04, 9.7416343842521791e-01 + }; + double T_3D_mark[3] = { + 2.0118012741488724e+02, -3.4890651052684677e+02,-2.9090717029324466e+01 + }; + double R_mark_3D[9] = { + 9.7414217523834201e-01, 6.4677677434932046e-03, 2.2584328726426117e-01, + -6.4917977205419487e-03, 9.9997872563826395e-01,-6.3626506196937213e-04, + -2.2584259780711641e-01, -8.4631630596682995e-04, 9.7416343842521791e-01 + }; + double T_mark_3D[3] = { + -1.8715145749760768e+02, 3.5018659904922669e+02, 7.3478870260969714e+01 + }; + + double R_3D_mark_20[9] = { + -9.7551111071544561e-01, 1.0398329224714965e-02,-2.1970422758802807e-01, + - 8.3582333026743106e-03,- 9.9991291186786824e-01, -1.0213158961794689e-02, + - 2.1979129374653508e-01, -8.1267108509691516e-03, 9.7551306693655782e-01, + }; + double T_3D_mark_20[3] = { + 5.7873032211966097e+02, 6.3216378913452741e+02, 6.5977504397803401e+01, + }; + double R_mark_3D_20[9] = { + -9.7551111071544561e-01, -8.3582333026743106e-03, -2.1979129374653508e-01, + 1.0398329224714965e-02, -9.9991291186786824e-01, -8.1267108509691516e-03, + -2.1970422758802807e-01, -1.0213158961794689e-02, 9.7551306693655782e-01 + }; + double T_mark_3D_20[3] = { + 5.8434291282050799e+02, 6.2662708685011432e+02, 6.9243970007470736e+01 + }; + + for (int grp = 0; grp <= 0; grp++) + { + char _scan_file[256]; + std::vector> originMarksBuff; + std::vector> currMarksBuff; + std::vector< SSX_BQAssemblyInfo> oiginPoseBuff; + std::vector< SSX_BQAssemblyInfo> computePoseBuff; + std::vector< SSX_BQAssemblyInfo> verifyPoseBuff; + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + //fidx =4; + char _scan_file[256]; + sprintf_s(_scan_file, "%sresult/LaserLine%d_corner_info.txt", dataPath[grp], fidx); + SSX_BQAssemblyInfo originPose_raw = _readPoseInfo(_scan_file); + SSX_BQAssemblyInfo originPose; + originPose.O = _pointRT(originPose_raw.O, R_3D_mark, T_3D_mark); + SSX_BQAssemblyInfo originPose_check; + originPose_check.O = _pointRT(originPose.O, R_mark_3D, T_mark_3D); + + sprintf_s(_scan_file, "%sresult/marks_%d.txt", dataPath[grp], fidx); + std::vector marks_origin; + _readMarkData(_scan_file, marks_origin); + + sprintf_s(_scan_file, "%sresult/marks_%d.txt", dataPath[grp+1], fidx); + std::vector marks_curr; + _readMarkData(_scan_file, marks_curr); + + //配对 + std::vector originMarkPos; + std::vector currMarkPos; + for (int i = 0; i < (int)marks_origin.size(); i++) + { + for (int j = 0; j < (int)marks_curr.size(); j++) + { + if (marks_origin[i].markID == marks_curr[j].markID) + { + SVzNL3DPoint pt_1 = { marks_origin[i].mark3D.x, marks_origin[i].mark3D.y, marks_origin[i].mark3D.z }; + SVzNL3DPoint pt_2 = { marks_curr[j].mark3D.x, marks_curr[j].mark3D.y, marks_curr[j].mark3D.z }; + originMarkPos.push_back(pt_1); + currMarkPos.push_back(pt_2); + } + } + } + + SSX_BQAssemblyInfo testPose_origin, testPose_verify; + + if (originMarkPos.size() >= 4) //同时使用最后一个作验证 + { + testPose_origin.O = originMarkPos.back(); + testPose_verify.O = currMarkPos.back(); + originMarkPos.pop_back(); + currMarkPos.pop_back(); + + originMarksBuff.push_back(originMarkPos); + currMarksBuff.push_back(currMarkPos); + + int errCode = 0; + SSX_BQAssemblyInfo currPose = sx_BQ_computeAssemblyInfoFromMark( + originPose, originMarkPos, currMarkPos, &errCode); + + SSX_BQAssemblyInfo compute_testPose = sx_BQ_computeAssemblyInfoFromMark( + testPose_origin, originMarkPos, currMarkPos, &errCode); +#if 0 + SSX_BQAssemblyInfo checkPose; + checkPose.O = _pointRT(originPose_raw.O, R_3D_mark_20, T_3D_mark_20); + SSX_BQAssemblyInfo checkPose_check; + checkPose_check.O = _pointRT(checkPose.O, R_mark_3D_20, T_mark_3D_20); +#endif + if (errCode != 0) + continue; + + //输出 + oiginPoseBuff.push_back(testPose_origin); + computePoseBuff.push_back(compute_testPose); + verifyPoseBuff.push_back(testPose_verify); + } + + } + sprintf_s(_scan_file, "%sresult/group_verify_data.txt", dataPath[grp]); + _wirteVerifyResult(_scan_file, + originMarksBuff, + currMarksBuff, + oiginPoseBuff, + computePoseBuff, + verifyPoseBuff); + } + +#endif +#if TEST_COMPUTE_CALIB_PARA + char _calib_datafile[256]; + sprintf_s(_calib_datafile, "F:/ShangGu/项目/冠钦_博清科技/组装/3D与Mark/21epic/调平/scan3D_0.txt"); + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + std::vector> scanData; + vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanData); + + + SVzNL3DRangeD roi; + roi.xRange.min = -DBL_MAX; + roi.xRange.max = DBL_MAX; + roi.yRange.min = -DBL_MAX; + roi.yRange.max = 580.0; + roi.zRange.min = 2380.0; + roi.zRange.max = 2460.0; + + std::vector> roiData; + _getRoiData_XYZ_vector(scanData, roiData, roi); + + lineNum = (int)scanData.size(); + if (scanData.size() > 0) + { + SSG_planeCalibPara calibPara = sx_BQ_getHoleBaseCalibPara(scanData); + //结果进行验证 + for (int i = 0; i < lineNum; i++) + { + if (i == 14) + int kkk = 1; + //行处理 + //调平，去除地面 + sx_BQ_lineDataR(scanData[i], calibPara.planeCalib, -1); // calibPara.planeHeight); + } + // + char calibFile[250]; + sprintf_s(calibFile, "F:/ShangGu/项目/冠钦_博清科技/组装/3D与Mark/21epic/ground_calib_para.txt"); + _outputCalibPara(calibFile, calibPara); + char _out_file[256]; + sprintf_s(_out_file, "F:/ShangGu/项目/冠钦_博清科技/组装/3D与Mark/21epic/调平/scanData_ground_calib.txt"); + int headNullLines = 0; + _outputScanDataFile_vector(_out_file, scanData, false, &headNullLines); + printf("%s: calib done!\n", _calib_datafile); + } +#endif + +#if TEST_COMPUTE_CORNER + const char* ver = wd_BQAssemblyPositionVersion(); + printf("ver:%s\n", ver); + + for (int grp = 0; grp <= 0; grp++) + { + SSG_planeCalibPara poseCalibPara; + //初始化成单位阵 + poseCalibPara.planeCalib[0] = 1.0; + poseCalibPara.planeCalib[1] = 0.0; + poseCalibPara.planeCalib[2] = 0.0; + poseCalibPara.planeCalib[3] = 0.0; + poseCalibPara.planeCalib[4] = 1.0; + poseCalibPara.planeCalib[5] = 0.0; + poseCalibPara.planeCalib[6] = 0.0; + poseCalibPara.planeCalib[7] = 0.0; + poseCalibPara.planeCalib[8] = 1.0; + poseCalibPara.planeHeight = -1.0; + for (int i = 0; i < 9; i++) + poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i]; + char calibFile[250]; + sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]); + poseCalibPara = _readCalibPara(calibFile); + + for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) + { + //fidx =4; + char _scan_file[256]; + sprintf_s(_scan_file, "%sscan3D_%d.txt", dataPath[grp], fidx); + std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + if (scanLines.size() == 0) + continue; + + //转成plyTxt格式 + //sprintf_s(_scan_file, "%s%d_ply_Hi229229.txt", dataPath[grp], fidx); + //wdSavePlyTxt(_scan_file, scanLines); + + long t1 = (long)GetTickCount64();//统计时间 + + +#if 0 + char _out_file[256]; + sprintf_s(_out_file, "%sscanData_%d_calib.txt", dataPath[grp], fidx); + int headNullLines = 0; + _outputScanDataFile_vector(_out_file, scanLines, false, &headNullLines); +#endif + + SSG_cornerParam cornerParam; + cornerParam.cornerTh = 30; //45度角 + cornerParam.scale = 5; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4; + cornerParam.minEndingGap_z = 20; + cornerParam.jumpCornerTh_1 = 10; //水平角度，小于此角度视为水平 + 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 = 10.0; + growParam.maxSkipDistance = 10.0; + growParam.zDeviation_max = 10.0;// algoParam.bagParam.bagH / 2; //袋子高度1/2 + growParam.minLTypeTreeLen = 100; //mm + growParam.minVTypeTreeLen = 100; //mm + SSX_BQAssemblyPara workpieceParam; + workpieceParam.type = 0; + workpieceParam.lineLen = 160.0; //直线段长度 + int errCode = 0; + SSX_BQAssemblyInfo assemblyPose = sx_BQ_computeAssemblyInfoFrom3D( + scanLines, + cornerParam, + filterParam, + growParam, + poseCalibPara, + workpieceParam, + &errCode); + long t2 = (long)GetTickCount64(); + printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); + //输出测试结果 + //sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx); + //_outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, assemblyPose); + sprintf_s(calibFile, "%sresult\\LaserLine%d_corner_info.txt", dataPath[grp], fidx); + _outputPoseInfo(calibFile, assemblyPose); + } + } +#endif } // 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单 diff --git a/BQ_assemblyPosition_test/BQ_assemblyPosition_test.vcxproj b/BQ_assemblyPosition_test/BQ_assemblyPosition_test.vcxproj index d84f2c0..15d0942 100644 --- a/BQ_assemblyPosition_test/BQ_assemblyPosition_test.vcxproj +++ b/BQ_assemblyPosition_test/BQ_assemblyPosition_test.vcxproj @@ -78,9 +78,13 @@ true + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) false + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) @@ -114,12 +118,15 @@ Level3 true - _DEBUG;_CONSOLE;%(PreprocessorDefinitions) + _CRT_SECURE_NO_WARNINGS;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) true + ..\..\thirdParty\opencv320\build\include; Console true + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + opencv_world320d.lib;BQ_assemblyPosition.lib;%(AdditionalDependencies) @@ -128,14 +135,17 @@ true true true - NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + _CRT_SECURE_NO_WARNINGS;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) true + ..\..\thirdParty\opencv320\build\include; Console true true true + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + opencv_world320.lib;BQ_assemblyPosition.lib;%(AdditionalDependencies) diff --git a/BinocularMarkCam_test/BinocularMarkCam_test.cpp b/BinocularMarkCam_test/BinocularMarkCam_test.cpp index 8182b14..3410cbc 100644 --- a/BinocularMarkCam_test/BinocularMarkCam_test.cpp +++ b/BinocularMarkCam_test/BinocularMarkCam_test.cpp @@ -12,22 +12,38 @@ #include #include -#define TEST_GROUP 3 +void _outputMarkData(char* fileName, std::vector& marks) +{ + std::ofstream sw(fileName); + char dataStr[250]; + for (int i = 0; i < (int)marks.size(); i++) + { + sprintf_s(dataStr, 250, "MarkID_%d: %g, %g, %g", marks[i].markID, marks[i].mark3D.x, marks[i].mark3D.y, marks[i].mark3D.z); + sw << dataStr << std::endl; + } + sw.close(); +} + +#define TEST_GROUP 6 int main(int argc, char** argv) { const char* dataPath[TEST_GROUP] = { "F:/标定采图/charucoMark图/MarkTest/", //0 "F:/标定采图/charucoMark图/Mark_13度/", - "F:/ShangGu/项目/冠钦_博清科技/组装/数据/mark测试/" }; + "F:/ShangGu/项目/冠钦_博清科技/组装/数据/mark测试/", + "F:/ShangGu/项目/冠钦_博清科技/组装/数据/现场Mark数据/", + "F:/ShangGu/项目/冠钦_博清科技/组装/3D与Mark/21epic/", + "F:/ShangGu/项目/冠钦_博清科技/组装/3D与Mark/20vizum/", + }; SVzNLRange fileIdx[TEST_GROUP] = { - {1,4}, {1,5}, {1,9} + {1,4}, {1,5}, {1,9}, {1, 1}, {2,33}, {2,33} }; const char* ver = wd_charuco3DMarkVersion(); printf("ver:%s\n", ver); - for (int grp = 2; grp < 3; grp++) + for (int grp = 4; grp < 6; grp++) { SWD_BQ_CharucoMarkInfo markInfo; markInfo.patternSize = cv::Size(3, 3); @@ -78,7 +94,7 @@ int main(int argc, char** argv) cv::Mat rightimg = cv::imread(filename, cv::IMREAD_GRAYSCALE); if (leftimg.empty() || rightimg.empty()) - break; + continue; double disparityOffset = 0; std::vector marks; @@ -89,6 +105,9 @@ int main(int argc, char** argv) disparityOffset, marks); + sprintf_s(calibFile, sizeof(calibFile), "%sresult/marks_%d.txt", dataPath[grp], index); + _outputMarkData(calibFile, marks); + printf("%s:\n", filename); for (int i = 0; i < marks.size(); i++) { diff --git a/HC_chanelSpaceMeasure_test/HC_chanelSpaceMeasure_test.cpp b/HC_chanelSpaceMeasure_test/HC_chanelSpaceMeasure_test.cpp new file mode 100644 index 0000000..0fd5b67 --- /dev/null +++ b/HC_chanelSpaceMeasure_test/HC_chanelSpaceMeasure_test.cpp @@ -0,0 +1,361 @@ +// gasFillingPortPosition_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。 +// + +#include +#include +#include +#include +#include +#include "direct.h" +#include +#include "channelSpaceMeasure_Export.h" +#include +#include +#include + +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>& 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>& scanData, SVzNLRangeD roi_y, std::vector>& 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>& 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, SSX_channelInfo channelInfo) +{ + std::ofstream sw(fileName); + + char dataStr[250]; + sprintf_s(dataStr, 250, "槽道间距: %g", channelInfo.channelSpace); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "槽道1: 宽_%g, 深_%g", channelInfo.channelWidth[0], channelInfo.channelDepth[0]); + sw << dataStr << std::endl; + sprintf_s(dataStr, 250, "槽道2: 宽_%g, 深_%g", channelInfo.channelWidth[1], channelInfo.channelDepth[1]); + sw << dataStr << std::endl; + sw.close(); +} + +void _outputRGBDScan_RGBD( + char* fileName, + std::vector>& scanLines +) +{ + int lineNum = (int)scanLines.size(); + std::ofstream sw(fileName); + int realLines = lineNum; + + 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 > 0) + { + rgb = objColor[pt3D->nPointIdx]; + size = 3; + } + 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; + } + } + sw.close(); +} + +#define TEST_GET_ROI_DATA 0 +#define TEST_COMPUTE_SPACE 1 +#define TEST_GROUP 1 +int main() +{ + const char* dataPath[TEST_GROUP] = { + "F:/ShangGu/项目/水木宏创/槽道间距测量/模拟数据/", //0 + }; + + SVzNLRange fileIdx[TEST_GROUP] = { + {1,9}, + }; + + const char* ver = wd_ChannelSPaceMeasureVersion(); + printf("ver:%s\n", ver); + +#if TEST_GET_ROI_DATA + + + for (int grp = 0; grp < TEST_GROUP; grp++) + { + SVzNLRangeD roi_y[10] = { + {0, 0}, + {-405.0, 510.0}, {-302.0,640.0}, {-290.0, 640.0}, + {-290.0, 670.0}, {-290.0, 640.0}, {-305.0, 660.0}, + {-296.0, 510.0}, {-300.0, 500.0}, {-330.0, 510.0} + }; + for (int fidx = 1; fidx <= 9; fidx++) + { + char _scan_file[256]; + sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx); + std::vector> scanData; + wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanData); + std::vector> scanROIData; + wd_gridScan_GetROIData(scanData, roi_y[fidx], scanROIData); + sprintf_s(_scan_file, "%sLaserData_%d_obj.txt", dataPath[grp], fidx); + _outputScanDataFile(_scan_file, scanROIData, 0, 0, 0); + } + } +#endif + +#if TEST_COMPUTE_SPACE + 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_obj.txt", dataPath[grp], fidx); + + std::vector> scanLines; + wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + + //转成plyTxt格式 + //sprintf_s(_scan_file, "%s%d_ply_Hi229229.txt", dataPath[grp], fidx); + //wdSavePlyTxt(_scan_file, scanLines); + + long t1 = (long)GetTickCount64();//统计时间 + + SSG_cornerParam cornerParam; + cornerParam.cornerTh = 45; //45度角 + cornerParam.scale = 15; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4; + cornerParam.minEndingGap_z = 50; + 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 = 100; //mm + growParam.minVTypeTreeLen = 100; //mm + + SSX_channelParam channelParam; + channelParam.channleSpaceRng = { 300, 800 }; + channelParam.channelWidthRng = { 5,30 }; + + bool isHorizonScan = true; //true:激光线平行槽道；false:激光线垂直槽道 + int errCode = 0; + SSX_channelInfo channelInfo = sx_channelSpaceMeasure( + scanLines, + isHorizonScan, //true:激光线平行槽道；false:激光线垂直槽道 + cornerParam, + filterParam, + growParam, + channelParam, + &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); + sprintf_s(_scan_file, "%sresult\\%d_fillingPort_info.txt", dataPath[grp], fidx); + _outputChanneltInfo(_scan_file, channelInfo); + } + } +#endif +} + +// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单 +// 调试程序: F5 或调试 >“开始调试”菜单 + +// 入门使用技巧: +// 1. 使用解决方案资源管理器窗口添加/管理文件 +// 2. 使用团队资源管理器窗口连接到源代码管理 +// 3. 使用输出窗口查看生成输出和其他消息 +// 4. 使用错误列表窗口查看错误 +// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目 +// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件 diff --git a/HC_chanelSpaceMeasure_test/HC_chanelSpaceMeasure_test.vcxproj b/HC_chanelSpaceMeasure_test/HC_chanelSpaceMeasure_test.vcxproj new file mode 100644 index 0000000..8fcc8f7 --- /dev/null +++ b/HC_chanelSpaceMeasure_test/HC_chanelSpaceMeasure_test.vcxproj @@ -0,0 +1,158 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + 16.0 + Win32Proj + {3ec47d19-2562-4303-82b9-6b1c93c5a37a} + HCchanelSpaceMeasuretest + 10.0 + HC_channelSpaceMeasure_test + + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + Application + true + v142 + Unicode + + + Application + false + v142 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + false + + + true + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + false + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + + Level3 + true + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + + + + + Level3 + true + true + true + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + + + Console + true + true + true + + + + + Level3 + true + _CRT_SECURE_NO_WARNINGS;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ..\..\thirdParty\opencv320\build\include; + + + Console + true + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + opencv_world320d.lib;HC_channelSpaceMeasure.lib;%(AdditionalDependencies) + + + + + Level3 + true + true + true + _CRT_SECURE_NO_WARNINGS;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + ..\..\thirdParty\opencv320\build\include; + + + Console + true + true + true + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + opencv_world320.lib;HC_channelSpaceMeasure.lib;%(AdditionalDependencies) + + + + + + + + + \ No newline at end of file diff --git a/HC_channelSpaceMeausre/HC_channelSpaceMeausre.vcxproj b/HC_channelSpaceMeausre/HC_channelSpaceMeausre.vcxproj new file mode 100644 index 0000000..11ceb14 --- /dev/null +++ b/HC_channelSpaceMeausre/HC_channelSpaceMeausre.vcxproj @@ -0,0 +1,173 @@ + + + + + Debug + Win32 + + + Release + Win32 + + + Debug + x64 + + + Release + x64 + + + + + + + + + + 16.0 + Win32Proj + {52a65444-8505-4fd5-9501-e2d297e2eb2c} + HCchannelSpaceMeausre + 10.0 + HC_channelSpaceMeasure + + + + DynamicLibrary + true + v142 + Unicode + + + DynamicLibrary + false + v142 + true + Unicode + + + DynamicLibrary + true + v142 + Unicode + + + DynamicLibrary + false + v142 + true + Unicode + + + + + + + + + + + + + + + + + + + + + true + + + false + + + true + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + false + $(SolutionDir)build\$(Platform)\$(Configuration)\ + ..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath) + + + + Level3 + true + WIN32;_DEBUG;HCCHANNELSPACEMEAUSRE_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + false + + + + + Level3 + true + true + true + WIN32;NDEBUG;HCCHANNELSPACEMEAUSRE_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + Use + pch.h + + + Windows + true + true + true + false + + + + + Level3 + true + _CRT_SECURE_NO_WARNINGS;_DEBUG;HCCHANNELSPACEMEAUSRE_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + ..\..\thirdParty\opencv320\build\include;%(AdditionalIncludeDirectories) + + + Windows + true + false + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories) + opencv_world320d.lib;baseAlgorithm.lib;%(AdditionalDependencies) + + + + + Level3 + true + true + true + _CRT_SECURE_NO_WARNINGS;NDEBUG;HCCHANNELSPACEMEAUSRE_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions) + true + NotUsing + pch.h + ..\..\thirdParty\opencv320\build\include;%(AdditionalIncludeDirectories) + + + Windows + true + true + true + false + ..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories) + opencv_world320.lib;baseAlgorithm.lib;%(AdditionalDependencies) + + + + + + \ No newline at end of file diff --git a/SG_Algorithm.sln b/SG_Algorithm.sln index 1484cdd..770057d 100644 --- a/SG_Algorithm.sln +++ b/SG_Algorithm.sln @@ -156,6 +156,10 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "BQ_assemblyPosition", "BQ_a EndProjectSection EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "BQ_assemblyPosition_test", "BQ_assemblyPosition_test\BQ_assemblyPosition_test.vcxproj", "{BC38D1E5-10CB-438B-AC72-6012303CE139}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + {0E62EEE4-ABB5-4364-A794-CCFFE8815426} = {0E62EEE4-ABB5-4364-A794-CCFFE8815426} + EndProjectSection EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "wheelArchHeigthMeasure", "wheelArchHeigthMeaure\wheelArchHeigthMeaure.vcxproj", "{CFE11556-106A-4216-BF62-FDA980528F7A}" ProjectSection(ProjectDependencies) = postProject @@ -169,8 +173,15 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "wheelArchHeigthMeasure_test EndProjectSection EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "HC_channelSpaceMeasure", "HC_channelSpaceMeausre\HC_channelSpaceMeausre.vcxproj", "{52A65444-8505-4FD5-9501-E2D297E2EB2C}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + EndProjectSection EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "HC_channelSpaceMeasure_test", "HC_chanelSpaceMeasure_test\HC_chanelSpaceMeasure_test.vcxproj", "{3EC47D19-2562-4303-82B9-6B1C93C5A37A}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + {52A65444-8505-4FD5-9501-E2D297E2EB2C} = {52A65444-8505-4FD5-9501-E2D297E2EB2C} + EndProjectSection EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution diff --git a/motorStatorPosition_test/motorStatorPosition_test.cpp b/motorStatorPosition_test/motorStatorPosition_test.cpp index 2963c4f..f2df5bc 100644 --- a/motorStatorPosition_test/motorStatorPosition_test.cpp +++ b/motorStatorPosition_test/motorStatorPosition_test.cpp @@ -299,185 +299,54 @@ void _outputScanDataFile_self(char* fileName, SVzNL3DLaserLine* scanData, int li sw.close(); } -SVzNL3DLaserLine* vzReadLaserScanPointFromFile_XYZ(const char* fileName, int* scanLineNum, float* scanV, - int* dataCalib, int* scanMaxStamp, int* canClockUnit) +void vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector>& scanData) { std::ifstream inputFile(fileName); std::string linedata; if (inputFile.is_open() == false) - return NULL; + return; - SVzNL3DLaserLine* _scanLines = NULL; - - int lines = 0; - int dataElements = 4; - int firstIndex = -1; - - int dataFileVer = DATA_VER_OLD; - std::getline(inputFile, linedata); //第一行 - int lineNum = 0; - if (0 == strncmp("LineNum:", linedata.c_str(), 8)) - { - dataFileVer = DATA_VER_NEW; - sscanf_s(linedata.c_str(), "LineNum:%d", &lines); - if (lines == 0) - return NULL; - lineNum = lines; - _scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)); - memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1)); - if (scanLineNum) - *scanLineNum = lines; - } - else if (0 == strncmp("LineNum_", linedata.c_str(), 8)) - { - dataFileVer = DATA_VER_OLD; - sscanf_s(linedata.c_str(), "LineNum_%d", &lines); - if (lines == 0) - return NULL; - lineNum = lines; - _scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1)); - memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1)); - if (scanLineNum) - *scanLineNum = lines; - } - if (_scanLines == NULL) - return NULL; - - int ptNum = 0; - int lineIdx = -1; + std::vector< SVzNL3DPosition> a_line; int ptIdx = 0; - SVzNL3DPosition* p3DPoint = NULL; - if (dataFileVer == DATA_VER_NEW) + while (getline(inputFile, linedata)) { - while (getline(inputFile, linedata)) + if (0 == strncmp("Line_", linedata.c_str(), 5)) { - if (0 == strncmp("ScanSpeed:", linedata.c_str(), 10)) + int ptSize = (int)a_line.size(); + if (ptSize > 0) { - double lineV = 0; - sscanf_s(linedata.c_str(), "ScanSpeed:%lf", &lineV); - if (scanV) - *scanV = (float)lineV; - } - else if (0 == strncmp("PointAdjust:", linedata.c_str(), 12)) - { - int ptAdjusted = 0; - sscanf_s(linedata.c_str(), "PointAdjust:%d", &ptAdjusted); - if (dataCalib) - *dataCalib = ptAdjusted; - } - else if (0 == strncmp("MaxTimeStamp:", linedata.c_str(), 13)) - { - unsigned int maxTimeStamp = 0; - unsigned int timePerStamp = 0; - sscanf_s(linedata.c_str(), "MaxTimeStamp:%u_%u", &maxTimeStamp, &timePerStamp); - if (scanMaxStamp) - *scanMaxStamp = maxTimeStamp; - if (canClockUnit) - *canClockUnit = timePerStamp; - } - else if (0 == strncmp("Line_", linedata.c_str(), 5)) - { - int lineIndex; - unsigned int timeStamp; - sscanf_s(linedata.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum); - if (firstIndex < 0) - firstIndex = lineIndex; - - lineIndex = lineIndex - firstIndex; - if ((lineIndex < 0) || (lineIndex >= lines)) - break; - - //new Line - lineIdx++; - if (ptNum > 0) - { - p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * ptNum); - memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * ptNum); - } - else - p3DPoint = NULL; - _scanLines[lineIdx].nPositionCnt = 0; - _scanLines[lineIdx].nTimeStamp = timeStamp; - _scanLines[lineIdx].p3DPosition = p3DPoint; - - } - 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); - int id = _scanLines[lineIdx].nPositionCnt; - if (id < ptNum) - { - p3DPoint[id].pt3D.x = X; - p3DPoint[id].pt3D.y = Y; - p3DPoint[id].pt3D.z = Z; - _scanLines[lineIdx].nPositionCnt = id + 1; - } + scanData.push_back(a_line); } + a_line.clear(); + ptIdx = 0; } - - } - else if (dataFileVer == DATA_VER_OLD) - { - while (getline(inputFile, linedata)) + else if (0 == strncmp("{", linedata.c_str(), 1)) { - if (0 == strncmp("DataElements_", linedata.c_str(), 13)) - { - sscanf_s(linedata.c_str(), "DataElements_%d", &dataElements); - if ((dataElements != 3) && (dataElements != 4)) - break; - } - if (0 == strncmp("LineV_", linedata.c_str(), 6)) - { - double lineV = 0; - sscanf_s(linedata.c_str(), "LineV_%lf", &lineV); - } - else if (0 == strncmp("Line_", linedata.c_str(), 5)) - { - int lineIndex; - unsigned int timeStamp; - sscanf_s(linedata.c_str(), "Line_%d_%u", &lineIndex, &timeStamp); -#if 0 - if (scanLineListTail == NULL) - firstIndex = lineIndex; -#endif - lineIndex = lineIndex - firstIndex; - if ((lineIndex < 0) || (lineIndex >= lines)) - break; - //new Line - //new Line - lineIdx++; - p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM); - memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM); - _scanLines[lineIdx].nPositionCnt = 0; - _scanLines[lineIdx].nTimeStamp = timeStamp; - _scanLines[lineIdx].p3DPosition = p3DPoint; - } - else if (0 == strncmp("(", linedata.c_str(), 1)) - { - float X, Y, Z; - int imageY = 0; - if (dataElements == 4) - sscanf_s(linedata.c_str(), "(%f,%f,%f,%d)", &X, &Y, &Z, &imageY); - else - sscanf_s(linedata.c_str(), "(%f,%f,%f)", &X, &Y, &Z); - int id = _scanLines[lineIdx].nPositionCnt; - if (id < VZ_LASER_LINE_PT_MAX_NUM) - { - p3DPoint[id].pt3D.x = X; - p3DPoint[id].pt3D.y = Y; - p3DPoint[id].pt3D.z = Z; - _scanLines[lineIdx].nPositionCnt = id + 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 _scanLines; + return; } typedef struct @@ -486,14 +355,16 @@ typedef struct int g; int b; }SG_color; -void _outputScanDataFile_RGBD_obj(char* fileName, SVzNL3DLaserLine* scanData, int lineNum, - float lineV, int maxTimeStamp, int clockPerSecond, SSG_boxCarDimension* dimen) +void _outputScanDataFile_RGBD_obj( + char* fileName, + std::vector>& scanData, + float lineV, int maxTimeStamp, int clockPerSecond, + std::vector& resultObjPositions) { + int lineNum = (int)scanData.size(); std::ofstream sw(fileName); int realLines = lineNum; - if (dimen) - realLines += 1; - + realLines++; sw << "LineNum:" << realLines << std::endl; sw << "DataType: 0" << std::endl; sw << "ScanSpeed:" << lineV << std::endl; @@ -517,97 +388,145 @@ void _outputScanDataFile_RGBD_obj(char* fileName, SVzNL3DLaserLine* scanData, in }; int size = 1; int nTimeStamp = 0; + double alpha = 0.8; for (int line = 0; line < lineNum; line++) { - sw << "Line_" << line << "_" << scanData[line].nTimeStamp << "_" << scanData[line].nPositionCnt << std::endl; - nTimeStamp = scanData[line].nTimeStamp; - for (int i = 0; i < scanData[line].nPositionCnt; i++) + int nPositionCnt = (int)scanData[line].size(); + sw << "Line_" << line << "_0_" << nPositionCnt << std::endl; + for (int i = 0; i < nPositionCnt; i++) { - SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i]; - if (pt3D->pt3D.z < 1e-4) - continue; - - int vType = pt3D->nPointIdx & 0xff; - int hType = vType >> 4; - int objId = (pt3D->nPointIdx >> 8) & 0xff; - vType = vType & 0x0f; - if (LINE_FEATURE_RIGHT_ANGLE_HR == vType) + SVzNL3DPosition& pt3D = scanData[line][i]; + int type = pt3D.nPointIdx; + if (1 == type) { - rgb = { 255, 97, 0 }; + rgb = { 0,255,0 }; + rgb.r = (int)((double)rgb.r * alpha); + rgb.g = (int)((double)rgb.g * alpha); + rgb.b = (int)((double)rgb.b * alpha); + size = 2; + } + else if (2 == type) + { + rgb = { 0,0,255 }; + rgb.r = (int)((double)rgb.r * alpha); + rgb.g = (int)((double)rgb.g * alpha); + rgb.b = (int)((double)rgb.b * alpha); + size = 2; + } + else if (3 == type) //轮眉 + { + rgb = { 255, 0, 0 }; size = 3; } - else if (LINE_FEATURE_RIGHT_ANGLE_HF == vType) + else if (4 == type) // { rgb = { 255, 255, 0 }; size = 3; } - else if (LINE_FEATURE_RIGHT_ANGLE_RH == vType) + else if (5 == type) // { - rgb = { 255, 0, 255 }; - size = 3; - } - else if (LINE_FEATURE_RIGHT_ANGLE_FH == vType) - { - rgb = { 160, 82, 45 }; - size = 3; - } - else if (LINE_FEATURE_RIGHT_ANGLE_HR == hType) - { - rgb = { 0, 0, 255 }; - size = 3; - } - else if (LINE_FEATURE_RIGHT_ANGLE_HF == hType) - { - rgb = { 0, 255, 255 }; - size = 3; - } - else if (LINE_FEATURE_RIGHT_ANGLE_RH == hType) - { - rgb = { 0, 255, 0 }; - size = 3; - } - else if (LINE_FEATURE_RIGHT_ANGLE_FH == hType) - { - rgb = { 85, 107, 47 }; + rgb = { 255, 255, 0 }; size = 3; } else { - rgb = { 200, 200, 200 }; + rgb = { 100, 100, 100 }; size = 1; } - float x = (float)pt3D->pt3D.x; - float y = (float)pt3D->pt3D.y; - float z = (float)pt3D->pt3D.z; + 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 (dimen) +#if 0 + //if (objOps.size() > 0) { - int ptNum = 4; + int ptNum = 3; sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << ptNum << std::endl; - for (int i = 0; i < 4; i++) - { - rgb = { 255, 0, 0 }; - size = 25; - float x = (float)dimen->endings[i].x; - float y = (float)dimen->endings[i].y; - float z = (float)dimen->endings[i].z; - sw << "{" << x << "," << y << "," << z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; - if (i == 0) - { - sw << "{" << x << "," << y << "," << z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; - } - } + rgb = { 255, 0, 0 }; + + size = 10; + float x = (float)wheelArcHeight.wheelArchPos.x; + float y = (float)wheelArcHeight.wheelArchPos.y; + float z = (float)wheelArcHeight.wheelArchPos.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + + x = (float)wheelArcHeight.wheelUpPos.x; + y = (float)wheelArcHeight.wheelUpPos.y; + z = (float)wheelArcHeight.wheelUpPos.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + + x = (float)wheelArcHeight.wheelDownPos.x; + y = (float)wheelArcHeight.wheelDownPos.y; + z = (float)wheelArcHeight.wheelDownPos.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + + x = (float)wheelArcHeight.wheelArchPos.x; + y = (float)wheelArcHeight.wheelArchPos.y; + z = (float)wheelArcHeight.wheelArchPos.z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; } + //画出方向线 + rgb = { 255, 0, 0 }; + size = 3; + int lineIdx = 0; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << (float)wheelArcHeight.arcLine[0].x << "," << (float)wheelArcHeight.arcLine[0].y << "," << (float)wheelArcHeight.arcLine[0].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << (float)wheelArcHeight.arcLine[1].x << "," << (float)wheelArcHeight.arcLine[1].y << "," << (float)wheelArcHeight.arcLine[1].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + + lineIdx++; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << (float)wheelArcHeight.upLine[0].x << "," << (float)wheelArcHeight.upLine[0].y << "," << (float)wheelArcHeight.upLine[0].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << (float)wheelArcHeight.upLine[1].x << "," << (float)wheelArcHeight.upLine[1].y << "," << (float)wheelArcHeight.upLine[1].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + + lineIdx++; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << (float)wheelArcHeight.downLine[0].x << "," << (float)wheelArcHeight.downLine[0].y << "," << (float)wheelArcHeight.downLine[0].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << (float)wheelArcHeight.downLine[1].x << "," << (float)wheelArcHeight.downLine[1].y << "," << (float)wheelArcHeight.downLine[1].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + + lineIdx++; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << (float)wheelArcHeight.centerLine[0].x << "," << (float)wheelArcHeight.centerLine[0].y << "," << (float)wheelArcHeight.centerLine[0].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << (float)wheelArcHeight.centerLine[1].x << "," << (float)wheelArcHeight.centerLine[1].y << "," << (float)wheelArcHeight.centerLine[1].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + + lineIdx++; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << (float)wheelArcHeight.arcLine[0].x << "," << (float)wheelArcHeight.arcLine[0].y << "," << (float)wheelArcHeight.arcLine[0].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << (float)wheelArcHeight.arcLine[1].x << "," << (float)wheelArcHeight.arcLine[1].y << "," << (float)wheelArcHeight.arcLine[1].z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; +#endif sw.close(); } @@ -884,26 +803,65 @@ int main() #if TEST_COMPUTE_GRASP_POS const char* dataPath[TEST_GROUP] = { - "E:\\ShangGu\\电机定子\\", //0 + "F:/ShangGu/电机定子/数据1/", //0 }; SVzNLRange fileIdx[TEST_GROUP] = { {1,1} }; - double camPoseR[9] = { - 1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0 }; - - SWD_statorParam algoParam; - algoParam.statorOuterD = 85.0; - algoParam.statorInnerD = 50.0; - - char _scan_file[256]; int endGroup = TEST_GROUP - 1; for (int grp = 0; grp <= endGroup; grp++) { + SSG_planeCalibPara poseCalibPara; + //初始化成单位阵 + poseCalibPara.planeCalib[0] = 1.0; + poseCalibPara.planeCalib[1] = 0.0; + poseCalibPara.planeCalib[2] = 0.0; + poseCalibPara.planeCalib[3] = 0.0; + poseCalibPara.planeCalib[4] = 1.0; + poseCalibPara.planeCalib[5] = 0.0; + poseCalibPara.planeCalib[6] = 0.0; + poseCalibPara.planeCalib[7] = 0.0; + poseCalibPara.planeCalib[8] = 1.0; + poseCalibPara.planeHeight = 2600.0; + for (int i = 0; i < 9; i++) + poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i]; + char calibFile[250]; +#if 0 + if (grp == 0) + { + sprintf_s(calibFile, "F:\\ShangGu\\粒径数据\\曝光\\3D数据\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + } +#endif + + SWD_statorParam statorParam; + statorParam.statorOuterD = 85.0; + statorParam.statorInnerD = 50.0; + + SWD_statorPositonParam algoParam; + memset(&algoParam, 0, sizeof(SWD_statorPositonParam)); + algoParam.cornerParam.cornerTh = 75; //45度角 + algoParam.cornerParam.scale = 5; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + algoParam.cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4; + algoParam.cornerParam.minEndingGap_z = 20; + algoParam.cornerParam.jumpCornerTh_1 = 20; //水平角度，小于此角度视为水平 + algoParam.cornerParam.jumpCornerTh_2 = 60; + SSG_outlierFilterParam filterParam; + algoParam.filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声 + algoParam.filterParam.outlierTh = 5; + SSG_treeGrowParam growParam; + algoParam.growParam.maxLineSkipNum = 10; + algoParam.growParam.yDeviation_max = 10.0; + algoParam.growParam.maxSkipDistance = 10.0; + algoParam.growParam.zDeviation_max = 10.0;// algoParam.bagParam.bagH / 2; //袋子高度1/2 + algoParam.growParam.minLTypeTreeLen = 100; //mm + algoParam.growParam.minVTypeTreeLen = 100; //mm + + SWD_nextOpParam refPos; + memset(&refPos, 0, sizeof(SWD_nextOpParam)); + refPos.cuttingZ = -1; //初始值，设为-1 for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { //fidx = 4; @@ -912,31 +870,37 @@ int main() int dataCalib = 0; int maxTimeStamp = 0; int clockPerSecond = 0; + + char _scan_file[256]; sprintf_s(_scan_file, "%sLaserLine%d_grid.txt", dataPath[grp], fidx); - SVzNL3DLaserLine* laser3DPoints = vzReadLaserScanPointFromFile_XYZ(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond); - if (laser3DPoints == NULL) + std::vector> scanLines; + vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines); + if (scanLines.size() == 0) continue; long t1 = GetTickCount64(); int errCode = 0; - std::vector resultOpPositions; - sg_motorStatorPosition( - laser3DPoints, - lineNum, + std::vector resultObjPositions; + SWD_statorOuterGrasper resultGraspPos; + wd_motorStatorPosition( + scanLines, + statorParam, + poseCalibPara, algoParam, + &refPos, //上一次给出的参考位置，同时输出下一次的参考位置 &errCode, - resultOpPositions); + resultObjPositions, + resultGraspPos); long t2 = GetTickCount64(); char _dbg_file[256]; #if 1 sprintf_s(_dbg_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx); - _outputScanDataFile_RGBD_obj(_dbg_file, laser3DPoints, lineNum, lineV, maxTimeStamp, clockPerSecond, NULL); - + _outputScanDataFile_RGBD_obj(_dbg_file,scanLines, 0,0,0, resultObjPositions); sprintf_s(_dbg_file, "%sresult\\LaserLine%d_result_img.png", dataPath[grp], fidx); cv::String imgName(_dbg_file); double rpy[3] = { -30, 15, 0 }; //{ 0,-45, 0 }; // - _genXOYProjectionImage(imgName, laser3DPoints, lineNum, rpy); + //_genXOYProjectionImage(imgName, laser3DPoints, lineNum, rpy); #endif printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); } diff --git a/sourceCode/BQ_assemblyPosition.cpp b/sourceCode/BQ_assemblyPosition.cpp index 81407a0..9945a82 100644 --- a/sourceCode/BQ_assemblyPosition.cpp +++ b/sourceCode/BQ_assemblyPosition.cpp @@ -12,13 +12,13 @@ const char* wd_BQAssemblyPositionVersion(void) return m_strVersion.c_str(); } -//һƽƽ +//Կװһƽƽ㷨ȡɨûп׵ɨ߽ //пһƽͲοƽƽ棬ߵƽеƽ //תΪƽƽ淨ΪֱIJ -SSG_planeCalibPara sx_BQ_getBaseCalibPara( +SSG_planeCalibPara sx_BQ_getHoleBaseCalibPara( std::vector< std::vector>& scanLines) { - return sg_getPlaneCalibPara2(scanLines); + return sg_getHolePlaneCalibPara(scanLines); } //̬ƽȥ @@ -66,6 +66,37 @@ SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFrom3D( return assemblyPose; } + std::vector> groundMask; + for (int line = 0, line_max = (int)scanLines.size(); line < line_max; line++) + { + //ƽȥ + std::vector& a_line = scanLines[line]; + std::vector a_lineMask; + a_lineMask.resize(a_line.size()); + for (int i = 0; i < a_line.size(); i++) + { + if ((line == 1000) && (i == 1439)) + int kkk = 1; + a_line[i].nPointIdx = i; + a_lineMask[i] = 0; + SVzNL3DPoint& a_pt = a_line[i].pt3D; + if (a_pt.z < 1e-4) + continue; + double x = a_pt.x * groundCalibPara.planeCalib[0] + a_pt.y * groundCalibPara.planeCalib[1] + a_pt.z * groundCalibPara.planeCalib[2]; + double y = a_pt.x * groundCalibPara.planeCalib[3] + a_pt.y * groundCalibPara.planeCalib[4] + a_pt.z * groundCalibPara.planeCalib[5]; + double z = a_pt.x * groundCalibPara.planeCalib[6] + a_pt.y * groundCalibPara.planeCalib[7] + a_pt.z * groundCalibPara.planeCalib[8]; + if ((groundCalibPara.planeHeight > 0) && (z > (groundCalibPara.planeHeight-5))) //ɵMask + { + a_lineMask[i] = 1; // a_line[i].nPointIdx = -1; //mark + } + a_pt.x = x; + a_pt.y = y; + a_pt.z = z; + a_line[i].pt3D = a_pt; + } + groundMask.push_back(a_lineMask); + } + //ʼͽĿհɨȥɨ߽ int validStartLine = -1; for (int i = 0; i < lineNum; i++) @@ -98,10 +129,11 @@ SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFrom3D( //ӦתǶ //ֱȡ + double contourWin = 40.0; //渽ĹյΪ˵ std::vector> jumpFeatures_v_raw; for (int line = 0; line < lineNum; line++) { - if (line == 250) + if (line == 1000) int kkk = 1; std::vector& lineData = scanLines[line]; @@ -113,11 +145,12 @@ SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFrom3D( std::vector line_features; int dataSize = (int)lineData.size(); - sg_getLineCornerFeature_BQ( - &lineData[0], - dataSize, + /// ȡϵĹյ˵㸽ĹյΪϸյ + sg_getLineContourCornerFeature_groundMask_BQ( + lineData, + groundMask[line], line, - groundCalibPara.planeHeight, + contourWin, //groundԵΧ cornerPara, //scaleͨȡbagH1/4 line_features); jumpFeatures_v_raw.push_back(line_features); @@ -130,10 +163,19 @@ SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFrom3D( } //ˮƽɨ + std::vector> jumpFeatures_h_raw; std::vector> hLines_raw; + std::vector> groundMask_hLines; hLines_raw.resize(linePtNum); + int lineNum_h_raw = (int)hLines_raw.size(); +#if 0 + hLines_raw.resize(linePtNum); + groundMask_hLines.resize(linePtNum); for (int i = 0; i < linePtNum; i++) + { hLines_raw[i].resize(lineNum); + groundMask_hLines[i].resize(lineNum); + } for (int line = 0; line < lineNum; line++) { for (int j = 0; j < linePtNum; j++) @@ -142,14 +184,13 @@ SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFrom3D( hLines_raw[j][line] = scanLines[line][j]; hLines_raw[j][line].pt3D.x = scanLines[line][j].pt3D.y; hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x; + groundMask_hLines[j][line] = groundMask[line][j]; } } //ˮƽarcȡ - std::vector> jumpFeatures_h_raw; - int lineNum_h_raw = (int)hLines_raw.size(); for (int line = 0; line < lineNum_h_raw; line++) { - if (line == 416) + if (line == 1906) int kkk = 1; std::vector& lineData = hLines_raw[line]; //˲˳쳣 @@ -158,55 +199,160 @@ SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFrom3D( std::vector line_features; int dataSize = (int)lineData.size(); - sg_getLineCornerFeature_BQ( - &hLines_raw[line][0], - dataSize, + + sg_getLineContourCornerFeature_groundMask_BQ( + hLines_raw[line], + groundMask_hLines[line], line, - groundCalibPara.planeHeight, + contourWin, //groundԵΧ cornerPara, //scaleͨȡbagH1/4 line_features); jumpFeatures_h_raw.push_back(line_features); } - +#endif //˳ - //ֱ߷ - std::vector v_trees; + //ʹþ෨ˮƽʹֱ + + std::vector> featureInfoMask; + std::vector> feature3DInfo; + featureInfoMask.resize(lineNum); + feature3DInfo.resize(lineNum); + for (int i = 0; i < lineNum; i++) + { + featureInfoMask[i].resize(linePtNum); + feature3DInfo[i].resize(linePtNum); + } + //Mask + double meanX = 0, meanY = 0, meanZ = 0; + int objPtSize = 0; for (int line = 0; line < lineNum; line++) { - bool isLastLine = false; - if (line == lineNum - 1) - isLastLine = true; - std::vector& a_lineJumpFeature = jumpFeatures_v_raw[line]; - if (a_lineJumpFeature.size() > 0) - int kkk = 1; - if (line == 202) - int kkk = 1; - sg_lineFeaturesGrowing( - line, - isLastLine, - a_lineJumpFeature, - v_trees, - growParam); + int featureNum = jumpFeatures_v_raw[line].size(); + for (int j = 0; j < featureNum; j++) + { + SVzNL2DPoint& a_pos = jumpFeatures_v_raw[line][j].jumpPos2D; + SSG_featureClusteringInfo a_mask; + memset(&a_mask, 0, sizeof(SSG_featureClusteringInfo)); + a_mask.featurType = 1; + a_mask.lineIdx = a_pos.x; + a_mask.ptIdx = a_pos.y; + featureInfoMask[a_pos.x][a_pos.y] = a_mask; + feature3DInfo[a_pos.x][a_pos.y] = scanLines[a_pos.x][a_pos.y].pt3D; + meanX += scanLines[a_pos.x][a_pos.y].pt3D.x; + meanY += scanLines[a_pos.x][a_pos.y].pt3D.y; + meanZ += scanLines[a_pos.x][a_pos.y].pt3D.z; + objPtSize++; + } } - - //ˮƽɨ˶ - std::vector h_trees; +#if 0 for (int line = 0; line < lineNum_h_raw; line++) { - if (line == 650) - int kkk = 1; - bool isLastLine = false; - if (line == lineNum_h_raw - 1) - isLastLine = true; - std::vector& a_lineJumpFeature = jumpFeatures_h_raw[line]; - sg_lineFeaturesGrowing( - line, - isLastLine, - a_lineJumpFeature, - h_trees, - growParam); + int featureNum = jumpFeatures_h_raw[line].size(); + for (int j = 0; j < featureNum; j++) + { + SVzNL2DPoint& a_pos = jumpFeatures_v_raw[line][j].jumpPos2D; + if (featureInfoMask[a_pos.y][a_pos.x].featurType == 0) + { + SSG_featureClusteringInfo a_mask; + memset(&a_mask, 0, sizeof(SSG_featureClusteringInfo)); + a_mask.featurType = 1; + a_mask.lineIdx = a_pos.y; + a_mask.ptIdx = a_pos.x; + featureInfoMask[a_pos.y][a_pos.x] = a_mask; + feature3DInfo[a_pos.y][a_pos.x] = scanLines[a_pos.y][a_pos.x].pt3D; + } + } + } +#endif + +#if 0 + // + //õ˼룬ع˼·иЧ + std::vector> clusters; //ֻ¼λ + std::vector clustersInfo; + int clusterID = 1; + int clusterCheckWin = 30; + for (int y = 0; y < linePtNum; y++) + { + for (int x = 0; x < lineNum; x++) + { + SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y]; + if ((0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //Ѿ + continue; + + SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y]; + SVzNL3DRangeD a_clusterRoi; + a_clusterRoi.xRange.min = a_feature3DValue.x; + a_clusterRoi.xRange.max = a_feature3DValue.x; + a_clusterRoi.yRange.min = a_feature3DValue.y; + a_clusterRoi.yRange.max = a_feature3DValue.y; + a_clusterRoi.zRange.min = a_feature3DValue.z; + a_clusterRoi.zRange.max = a_feature3DValue.z; + + SVzNL2DPoint a_seedPos = { x, y }; + std::vector< SVzNL2DPoint> a_cluster; + a_cluster.push_back(a_seedPos); + wd_gridPointClustering( + featureInfoMask,//int¼ǺclusterIDһflag + feature3DInfo,//double,¼Ϣ + clusterCheckWin, // + growParam,// + clusterID, //ǰClusterID + a_cluster, //result + a_clusterRoi + ); + clusters.push_back(a_cluster); + SWD_clustersInfo a_info; + a_info.clusterIdx = clusterID; + a_info.ptSize = (int)a_cluster.size(); + a_info.roi3D = a_clusterRoi; + clustersInfo.push_back(a_info); + clusterID++; + } + } + //ȡľΪ߽ + int clusterSize = clusters.size(); + if(clusterSize == 0) + { + *errCode = SX_ERR_ZERO_OBJ; + return assemblyPose; } + int objPtSize = clusters[0].size(); + int objClusterID = 0; + for (int i = 1; i < clusterSize; i++) + { + if (clusters[i].size() > objPtSize) + { + objPtSize = (int)clusters[i].size(); + objClusterID = i; + } + } + if (objPtSize == 0) + { + *errCode = SX_ERR_ZERO_OBJ; + return assemblyPose; + } + + double meanX = 0, meanY = 0, meanZ = 0; + for (int i = 0; i < objPtSize; i++) + { + int objLineIdx = clusters[objClusterID][i].x; + int objPtIdx = clusters[objClusterID][i].y; + scanLines[objLineIdx][objPtIdx].nPointIdx = 1; + meanX += scanLines[objLineIdx][objPtIdx].pt3D.x; + meanY += scanLines[objLineIdx][objPtIdx].pt3D.y; + meanZ += scanLines[objLineIdx][objPtIdx].pt3D.z; + } +#endif + meanX = meanX / objPtSize; + meanY = meanY / objPtSize; + meanZ = meanZ / objPtSize; + assemblyPose.O.x = meanX; + assemblyPose.O.y = meanY; + assemblyPose.O.z = meanZ; + +#if 0 std::vector polarPoints; for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++) { @@ -407,20 +553,21 @@ SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFrom3D( std::vector sidePts_2; std::vector sidePts_3; std::vector sidePts_4; +#endif return assemblyPose; } //Mark㹤λú̬ SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFromMark( - SSX_BQAssemblyInfo originPos, - std::vector originMarkPos, - std::vector currMarkPos, + SSX_BQAssemblyInfo& originPos, + std::vector& originMark, + std::vector& currMark, int* errCode) { *errCode = 0; SSX_BQAssemblyInfo resultPos; memset(&resultPos, 0, sizeof(SSX_BQAssemblyInfo)); - if ((originMarkPos.size() < 3) || (currMarkPos.size() < 3)) + if ((originMark.size() < 3) || (currMark.size() < 3) || (originMark.size() != currMark.size())) { *errCode = SX_ERR_INVLID_MARK_NUM; return resultPos; @@ -428,13 +575,42 @@ SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFromMark( cv::Mat R, T; //תƽ cv::Point3d C_origin, C_curr; // + + std::vector originMarkPos; + originMarkPos.resize(originMark.size()); + std::vector currMarkPos; + currMarkPos.resize(originMark.size()); + for (int i = 0; i < (int)originMark.size(); i++) + { + originMarkPos[i] = { originMark[i].x, originMark[i].y, originMark[i].z }; + currMarkPos[i] = { currMark[i].x, currMark[i].y, currMark[i].z}; + } caculateRT(originMarkPos, currMarkPos, R, T, C_origin, C_curr); + std::vector originMarkPos_RT; + originMarkPos_RT.resize(originMarkPos.size()); + for (int i = 0; i < (int)originMark.size(); i++) + { + cv::Point3d RT_pt; + pointRT(R, T, C_origin, C_curr, originMarkPos[i], RT_pt); + originMarkPos_RT[i] = RT_pt; + } // 5. Pn - pointRT(R, T, C_origin, C_curr, originPos.O, resultPos.O); - pointRT(R, T, C_origin, C_curr, originPos.X, resultPos.X); - pointRT(R, T, C_origin, C_curr, originPos.Y, resultPos.Y); - pointRT(R, T, C_origin, C_curr, originPos.Z, resultPos.Z); + cv::Point3d O = { originPos.O.x, originPos.O.y, originPos.O.z}; + cv::Point3d X = { originPos.X.x, originPos.X.y, originPos.X.z }; + cv::Point3d Y = { originPos.Y.x, originPos.Y.y, originPos.Y.z }; + cv::Point3d Z = { originPos.Z.x, originPos.Z.y, originPos.Z.z }; + + cv::Point3d RO, RX, RY, RZ; + pointRT(R, T, C_origin, C_curr, O, RO); + pointRT(R, T, C_origin, C_curr, X, RX); + pointRT(R, T, C_origin, C_curr, Y, RY); + pointRT(R, T, C_origin, C_curr, Z, RZ); + resultPos.O = { RO.x, RO.y, RO.z }; + resultPos.X = { RX.x, RX.y, RX.z }; + resultPos.Y = { RY.x, RY.y, RY.z }; + resultPos.Z = { RZ.x, RZ.y, RZ.z }; + return resultPos; } diff --git a/sourceCode/BQ_assemblyPosition_Export.h b/sourceCode/BQ_assemblyPosition_Export.h index d431473..6dddc25 100644 --- a/sourceCode/BQ_assemblyPosition_Export.h +++ b/sourceCode/BQ_assemblyPosition_Export.h @@ -14,10 +14,10 @@ typedef struct typedef struct { int type; // - cv::Point3d O; //ĵ - cv::Point3d X; //ϵOX - cv::Point3d Y; //ϵOY - cv::Point3d Z; //ϵOZ + SVzNL3DPoint O; //ĵ + SVzNL3DPoint X; //ϵOX + SVzNL3DPoint Y; //ϵOY + SVzNL3DPoint Z; //ϵOZ }SSX_BQAssemblyInfo; //汾 @@ -26,7 +26,7 @@ SG_APISHARED_EXPORT const char* wd_BQAssemblyPositionVersion(void); //һƽƽ //пһƽͲοƽƽ棬ߵƽеƽ //תΪƽƽ淨ΪֱIJ -SG_APISHARED_EXPORT SSG_planeCalibPara sx_BQ_getBaseCalibPara( +SG_APISHARED_EXPORT SSG_planeCalibPara sx_BQ_getHoleBaseCalibPara( std::vector< std::vector>& scanLines); //̬ƽȥ @@ -36,7 +36,7 @@ SG_APISHARED_EXPORT void sx_BQ_lineDataR( double groundH); //ȡǵ㼰λϢ -SG_APISHARED_EXPORT SSX_BQAssemblyInfo sx_BQ_getAssemblyInfo( +SG_APISHARED_EXPORT SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFrom3D( std::vector< std::vector>& scanLines, const SSG_cornerParam cornerPara, const SSG_outlierFilterParam filterParam, @@ -45,5 +45,10 @@ SG_APISHARED_EXPORT SSX_BQAssemblyInfo sx_BQ_getAssemblyInfo( SSX_BQAssemblyPara assemblyParam, int* errCode); - +//Mark㹤λú̬ +SG_APISHARED_EXPORT SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFromMark( + SSX_BQAssemblyInfo& originPos, + std::vector& originMark, + std::vector& currMark, + int* errCode); diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index 17d2028..1155a66 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -67,6 +67,14 @@ SG_APISHARED_EXPORT void sg_maskData_getLineCornerFeature( const SSG_cornerParam cornerPara, //scaleͨȡbagH1/4 std::vector& features); +//ȡGapGapCornerɵ +SG_APISHARED_EXPORT void sg_getLineGapFeature( + std::vector& lineData, //ɨ + int lineIdx, //ǰɨ + const SSG_cornerParam cornerPara, + const SVzNLRangeD gapParam, + std::vector& line_gaps); + SG_APISHARED_EXPORT void sg_getLineCornerFeature_BQ( SVzNL3DPosition* lineData, int dataSize, @@ -75,6 +83,24 @@ SG_APISHARED_EXPORT void sg_getLineCornerFeature_BQ( const SSG_cornerParam cornerPara, std::vector& line_features); +/// +/// ȡϵĹյ˵㸽ĹյΪϸյ +/// Segcornerȡ +/// nPointIdx¶Feature +/// 㷨̣ +/// 1ǰǺͺ +/// 2սǣ˳ʱΪʱΪ +/// 3սǵļֵ +/// 4жϹսǷΪ +/// +SG_APISHARED_EXPORT void sg_getLineContourCornerFeature_groundMask_BQ( + std::vector& lineData, + std::vector groundMask, + int lineIdx, + double contourWin, //groundԵΧ + const SSG_cornerParam cornerPara, //scaleͨȡbagH1/4 + std::vector& line_features); + SG_APISHARED_EXPORT void wd_getLineDataIntervals( std::vector& lineData, const SSG_lineSegParam lineSegPara, @@ -218,6 +244,14 @@ SG_APISHARED_EXPORT void sg_lineFeaturesGrowing( std::vector& trees, SSG_treeGrowParam growParam); +//gap +SG_APISHARED_EXPORT void sg_lineGapsGrowing( + int lineIdx, + bool isLastLine, + std::vector& features, + std::vector& trees, + SSG_treeGrowParam growParam); + //SG_APISHARED_EXPORT void sg_getTreeROI(SSG_featureTree* a_tree); //ending diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index 3102cf8..a54cdf2 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -265,6 +265,18 @@ typedef struct int angleChkScalePos; //ڼangleCheckٶ }SSG_featureTree; +typedef struct +{ + int treeState; + int treeType; + int sLineIdx; + int eLineIdx; + double tree_value; + SSG_ROIRectD roi; + std::vector< SSG_basicFeatureGap> treeNodes; + int angleChkScalePos; //ڼangleCheckٶ +}SSG_gapFeatureTree; + typedef struct { int treeState; @@ -531,4 +543,5 @@ typedef struct int clusterIdx; int ptSize; SVzNL3DRangeD roi3D; + SVzNLRect roi2D; }SWD_clustersInfo; diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index 485ed9c..4aeb206 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -2057,6 +2057,161 @@ SSG_planeCalibPara sg_getPlaneCalibPara2( return planePara; } +//Կװһƽƽȡ׵ɨ߽ƽ +//пһƽͲοƽƽ棬ߵƽеƽ +//תΪƽƽ淨ΪֱIJ +SSG_planeCalibPara sg_getHolePlaneCalibPara( + std::vector< std::vector>& scanLines) +{ + //óʼ + double initCalib[9] = { + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0 }; + SSG_planeCalibPara planePara; + for (int i = 0; i < 9; i++) + planePara.planeCalib[i] = initCalib[i]; + planePara.planeHeight = -1.0; + + SSG_lineSegParam segParam; + segParam.segGapTh_y = 1.0; + segParam.segGapTh_z = 1.0; + int lineNum = (int)scanLines.size(); + std::vector validLines; + for (int line = 0; line < lineNum; line++) + { + //ȥ + std::vector segs; + wd_getLineDataIntervals(scanLines[line], segParam, segs); + if (segs.size() == 1) + validLines.push_back(line); + } + + //ȡ + std::vector Points3ds; + for (int vline = 0; vline < (int)validLines.size(); vline++) + { + int line = validLines[vline]; + int nPositionCnt = (int)scanLines[line].size(); + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPosition* pt3D = &scanLines[line][i]; + if (pt3D->pt3D.z < 1e-4) + continue; + + cv::Point3f a_vldPt; + a_vldPt.x = (float)pt3D->pt3D.x; + a_vldPt.y = (float)pt3D->pt3D.y; + a_vldPt.z = (float)pt3D->pt3D.z; + Points3ds.push_back(a_vldPt); + } + } + //ƽ + std::vector planceFunc; + vzCaculateLaserPlane(Points3ds, planceFunc); + +#if 1 //תתʹԪ + Vector3 a = Vector3(planceFunc[0], planceFunc[1], planceFunc[2]); + Vector3 b = Vector3(0, 0, -1.0); + Quaternion quanPara = rotationBetweenVectors(a, b); + + RotationMatrix rMatrix; + quaternionToMatrix(quanPara, rMatrix.data); + //㷴ת + Quaternion invQuanPara = rotationBetweenVectors(b, a); + RotationMatrix invMatrix; + quaternionToMatrix(invQuanPara, invMatrix.data); +#else //ƽķŷǣת + // + SSG_EulerAngles eulerPra = planeNormalToEuler(planceFunc[0], planceFunc[1], planceFunc[2]); + //У + eulerPra.roll = eulerPra.roll; + eulerPra.pitch = eulerPra.pitch; + eulerPra.yaw = eulerPra.yaw; + RotationMatrix rMatrix = eulerToRotationMatrix(eulerPra.yaw, eulerPra.pitch, eulerPra.roll); +#endif + + planePara.planeCalib[0] = rMatrix.data[0][0]; + planePara.planeCalib[1] = rMatrix.data[0][1]; + planePara.planeCalib[2] = rMatrix.data[0][2]; + planePara.planeCalib[3] = rMatrix.data[1][0]; + planePara.planeCalib[4] = rMatrix.data[1][1]; + planePara.planeCalib[5] = rMatrix.data[1][2]; + planePara.planeCalib[6] = rMatrix.data[2][0]; + planePara.planeCalib[7] = rMatrix.data[2][1]; + planePara.planeCalib[8] = rMatrix.data[2][2]; + + planePara.invRMatrix[0] = invMatrix.data[0][0]; + planePara.invRMatrix[1] = invMatrix.data[0][1]; + planePara.invRMatrix[2] = invMatrix.data[0][2]; + planePara.invRMatrix[3] = invMatrix.data[1][0]; + planePara.invRMatrix[4] = invMatrix.data[1][1]; + planePara.invRMatrix[5] = invMatrix.data[1][2]; + planePara.invRMatrix[6] = invMatrix.data[2][0]; + planePara.invRMatrix[7] = invMatrix.data[2][1]; + planePara.invRMatrix[8] = invMatrix.data[2][2]; + +#if 0 //test: ij˻ǵλ + double testMatrix[3][3]; + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + testMatrix[i][j] = 0; + for (int m = 0; m < 3; m++) + testMatrix[i][j] += invMatrix.data[i][m] * rMatrix.data[m][j]; + } + } +#endif + //ݽת + SVzNLRangeD calibZRange = { 0, -1 }; + SVzNLRangeD topZRange = { 0, -1 }; + double sumMeanZ = 0; + int sumSize = 0; + for (int i = 0, i_max = (int)Points3ds.size(); i < i_max; i++) + { + //z + if (topZRange.max < topZRange.min) + { + topZRange.min = Points3ds[i].z; + topZRange.max = Points3ds[i].z; + } + else + { + if (topZRange.min > Points3ds[i].z) + topZRange.min = Points3ds[i].z; + if (topZRange.max < Points3ds[i].z) + topZRange.max = Points3ds[i].z; + } + cv::Point3f a_calibPt; + a_calibPt.x = (float)(Points3ds[i].x * planePara.planeCalib[0] + Points3ds[i].y * planePara.planeCalib[1] + Points3ds[i].z * planePara.planeCalib[2]); + a_calibPt.y = (float)(Points3ds[i].x * planePara.planeCalib[3] + Points3ds[i].y * planePara.planeCalib[4] + Points3ds[i].z * planePara.planeCalib[5]); + a_calibPt.z = (float)(Points3ds[i].x * planePara.planeCalib[6] + Points3ds[i].y * planePara.planeCalib[7] + Points3ds[i].z * planePara.planeCalib[8]); + //z + if (calibZRange.max < calibZRange.min) + { + calibZRange.min = a_calibPt.z; + calibZRange.max = a_calibPt.z; + sumMeanZ += a_calibPt.z; + sumSize++; + } + else + { + if (calibZRange.min > a_calibPt.z) + calibZRange.min = a_calibPt.z; + if (calibZRange.max < a_calibPt.z) + calibZRange.max = a_calibPt.z; + sumMeanZ += a_calibPt.z; + sumSize++; + } + } + if (sumSize > 0) + sumMeanZ = sumMeanZ / (double)sumSize; + planePara.planeHeight = sumMeanZ; // calibZRange.min; + + return planePara; +} + //ˮƽװֱɨģʽƽ SSG_planeCalibPara sg_HCameraVScan_getGroundCalibPara( std::vector< std::vector>& scanLines) @@ -2826,7 +2981,7 @@ void pointRT(const cv::Mat& R, const cv::Mat& T, Eigen::Vector3d result = _R * (vec_pt - vec_origin) + vec_rtOrigin; rtPt.x = result(0); - rtPt.x = result(1); - rtPt.x = result(2); + rtPt.y = result(1); + rtPt.z = result(2); return; } diff --git a/sourceCode/SG_featureGrow.cpp b/sourceCode/SG_featureGrow.cpp index 131ec8a..5c54751 100644 --- a/sourceCode/SG_featureGrow.cpp +++ b/sourceCode/SG_featureGrow.cpp @@ -732,6 +732,112 @@ void sg_lineFeaturesGrowing( } } +//featuretreesѰҺʵûкʵ㣬 false +//ûʹȫƥ䡣һfeatureһƥϣƥɡûʹƥ䡣 +bool _gapGrowing( + SSG_basicFeatureGap& a_gap, + const int lineIdx, + std::vector& trees, + SSG_treeGrowParam growParam) +{ + for (int i = 0, i_max = (int)trees.size(); i < i_max; i++) + { + SSG_gapFeatureTree& a_tree = trees[i]; + if (TREE_STATE_DEAD == a_tree.treeState) + continue; + // + SSG_basicFeatureGap last_node = a_tree.treeNodes.back(); + if (last_node.lineIdx == a_gap.lineIdx) //xΪlineIdxͬһɨϵIJ + continue; + + double last_cx = (last_node.gapPt_0.pt3D.x + last_node.gapPt_1.pt3D.x) / 2; + double last_cy = (last_node.gapPt_0.pt3D.y + last_node.gapPt_1.pt3D.y) / 2; + double last_cz = (last_node.gapPt_0.pt3D.z + last_node.gapPt_1.pt3D.z) / 2; + //ж + double curr_cx = (a_gap.gapPt_0.pt3D.x + a_gap.gapPt_1.pt3D.x) / 2; + double curr_cy = (a_gap.gapPt_0.pt3D.y + a_gap.gapPt_1.pt3D.y) / 2; + double curr_cz = (a_gap.gapPt_0.pt3D.z + a_gap.gapPt_1.pt3D.z) / 2; + double y_diff = abs(curr_cy - last_cy); + double z_diff = abs(curr_cz - last_cz); + int line_diff = abs(a_gap.lineIdx - last_node.lineIdx); + double x_diff = abs(curr_cx - last_cx); + if ((y_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) && + ((line_diff < growParam.maxLineSkipNum) || (x_diff < growParam.maxSkipDistance)) ) + { + a_tree.eLineIdx = lineIdx; + a_tree.treeNodes.push_back(a_gap); + return true; + } + } + return false; +} + + +//Gapǵ򵥵½ߵûмȥ) +bool _invalidateGapTrees(SSG_gapFeatureTree& a_tree, double minLTypeTreeLen, double minVTypeTreeLen) +{ + SSG_basicFeatureGap& first_node = a_tree.treeNodes[0]; + double first_cx = (first_node.gapPt_0.pt3D.x + first_node.gapPt_1.pt3D.x) / 2; + double first_cy = (first_node.gapPt_0.pt3D.y + first_node.gapPt_1.pt3D.y) / 2; + SSG_basicFeatureGap& last_node = a_tree.treeNodes[a_tree.treeNodes.size() - 1]; + double last_cx = (last_node.gapPt_0.pt3D.x + last_node.gapPt_1.pt3D.x) / 2; + double last_cy = (last_node.gapPt_0.pt3D.y + last_node.gapPt_1.pt3D.y) / 2; + double len = sqrt(pow(first_cx - last_cx, 2) + pow(first_cy - last_cy, 2)); + if (len <= minLTypeTreeLen) + return false; + else + return true; +} + +void sg_lineGapsGrowing( + int lineIdx, + bool isLastLine, + std::vector& features, + std::vector& trees, + SSG_treeGrowParam growParam) +{ + if (features.size() > 0) + { + for (int j = 0, j_max = (int)features.size(); j < j_max; j++) + { + SSG_basicFeatureGap& a_feature = features[j]; + if (a_feature.lineIdx == 207) + int kkk = 1; + bool isMatched = _gapGrowing(a_feature, lineIdx, trees, growParam); + if (false == isMatched) + { + //µ + SSG_gapFeatureTree a_newTree; + a_newTree.treeNodes.push_back(a_feature); + a_newTree.treeState = TREE_STATE_ALIVE; + a_newTree.treeType = 1; + a_newTree.sLineIdx = lineIdx; + a_newTree.eLineIdx = lineIdx; + a_newTree.tree_value = 0; + trees.push_back(a_newTree); + } + } + } + //ֹͣ,١ + //ڵΪ1Ƴ + int m_max = (int)trees.size(); + for (int m = m_max - 1; m >= 0; m--) //ӺǰɾӰѭ + { + if (TREE_STATE_ALIVE == trees[m].treeState) + { + int line_diff = abs(lineIdx - trees[m].treeNodes.back().lineIdx); + if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) || + (true == isLastLine)) + { + trees[m].treeState = TREE_STATE_DEAD; + bool isValid = _invalidateGapTrees(trees[m], growParam.minLTypeTreeLen, growParam.minVTypeTreeLen); + if (false == isValid) + trees.erase(trees.begin() + m); + } + } + } +} + void sg_getEndingGrowingTrees( std::vector& lineEndings, SVzNL3DLaserLine* laser3DPoints, diff --git a/sourceCode/SG_lineFeature.cpp b/sourceCode/SG_lineFeature.cpp index a58b704..3eec456 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -2121,6 +2121,972 @@ void sg_getLineCornerFeature_BQ( return; } +/// +/// ȡϵĹյ˵㸽ĹյΪϸյ +/// Segcornerȡ +/// nPointIdx¶Feature +/// 㷨̣ +/// 1ǰǺͺ +/// 2սǣ˳ʱΪʱΪ +/// 3սǵļֵ +/// 4жϹսǷΪ +/// +void sg_getLineContourCornerFeature_groundMask_BQ( + std::vector& lineData, + std::vector groundMask, + int lineIdx, + double contourWin, //groundԵΧ + const SSG_cornerParam cornerPara, //scaleͨȡbagH1/4 + std::vector& line_features) +{ + int dataSize = (int)lineData.size(); + //ȥ + std::vector< SVzNL3DPosition> vldPts; + std::vector< int> vldPtSegIdx; + std::vector segs; + std::vector backIndexing; + backIndexing.resize(dataSize); + + int runIdx = 1; + SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx + double pre_z = 0; + double pre_y = 0; + for (int i = 0; i < dataSize; i++) + { + if (i == 370) + int kkk = 1; + if (lineData[i].pt3D.z > 1e-4) + { + if (a_run.len < 0) + { + a_run.start = i; + a_run.len = 1; + a_run.value = i; + } + else + { + double z_diff = abs(lineData[i].pt3D.z - pre_z); + double y_diff = abs(lineData[i].pt3D.y - pre_y); + if ((z_diff < cornerPara.minEndingGap_z) && (y_diff < cornerPara.minEndingGap)) + { + a_run.len = i - a_run.start + 1; + a_run.value = i; + } + else + { + a_run.value = runIdx; + runIdx++; + segs.push_back(a_run); + + a_run.start = i; + a_run.len = 1; + a_run.value = i; + } + } + int bIdx = (int)vldPts.size(); + backIndexing[i] = bIdx; + vldPts.push_back(lineData[i]); + vldPtSegIdx.push_back(runIdx); + + pre_z = lineData[i].pt3D.z; + pre_y = lineData[i].pt3D.y; + } + } + if (a_run.len > 0) + segs.push_back(a_run); + + //ǰǺͺ + std::vector< SSG_pntDirAngle> corners; + corners.resize(vldPts.size()); + for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) + { + if ((lineIdx == 399) && (i == 419)) + int kkk = 1; + //ǰѰ + int pre_i = -1; + for (int j = i - 1; j >= 0; j--) + { + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if ((dist >= cornerPara.scale) && (vldPtSegIdx[i] == vldPtSegIdx[j])) + { + pre_i = j; + break; + } + } + //Ѱ + int post_i = -1; + for (int j = i + 1; j < i_max; j++) + { + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if ((dist >= cornerPara.scale) && (vldPtSegIdx[i] == vldPtSegIdx[j])) + { + post_i = j; + break; + } + } + //ս + double tanValue_pre = 0; + if (pre_i >= 0) + tanValue_pre = (vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z) / abs(vldPts[i].pt3D.y - vldPts[pre_i].pt3D.y); + double tanValue_post = 0; + if (post_i >= 0) + tanValue_post = (vldPts[post_i].pt3D.z - vldPts[i].pt3D.z) / abs(vldPts[post_i].pt3D.y - vldPts[i].pt3D.y); + double forwardAngle = atan(tanValue_post) * 180.0 / PI; + double backwardAngle = atan(tanValue_pre) * 180.0 / PI; + + if ((pre_i < 0) && (post_i < 0)) + { + corners[i].pntIdx = -1; + corners[i].forwardAngle = 0; + corners[i].backwardAngle = 0; + corners[i].corner = 0; + corners[i].forwardDiffZ = 0; + corners[i].backwardDiffZ = 0; + } + else + { + corners[i].pntIdx = i; + corners[i].forwardAngle = forwardAngle; + corners[i].backwardAngle = backwardAngle; + if ((pre_i >= 0) && (post_i >= 0)) + { + corners[i].corner = -(forwardAngle - backwardAngle); //ͼϵϵy෴С- + corners[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z; + corners[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z; + } + else + { + corners[i].corner = 0; + corners[i].forwardDiffZ = 0; + corners[i].backwardDiffZ = 0; + } + } + } + + //սǼֵ + int _state = 0; + int pre_i = -1; + int sEdgePtIdx = -1; + int eEdgePtIdx = -1; + SSG_pntDirAngle* pre_data = NULL; + std::vector< SSG_pntDirAngle> cornerPeakP; + std::vector< SSG_pntDirAngle> cornerPeakM; + for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) + { + if (i == 275) + int kkk = 1; + SSG_pntDirAngle* curr_data = &corners[i]; + if (curr_data->pntIdx < 0) + { + if (i == i_max - 1) //һ + { + if (1 == _state) // + { + + cornerPeakP.push_back(corners[eEdgePtIdx]); + } + else if (2 == _state) //½ + { + cornerPeakM.push_back(corners[eEdgePtIdx]); + } + } + continue; + } + + if (NULL == pre_data) + { + sEdgePtIdx = i; + eEdgePtIdx = i; + pre_data = curr_data; + pre_i = i; + continue; + } + + eEdgePtIdx = i; + double cornerDiff = curr_data->corner - pre_data->corner; + switch (_state) + { + case 0: //̬ + if (cornerDiff < 0) //½ + { + _state = 2; + } + else if (cornerDiff > 0) // + { + _state = 1; + } + break; + case 1: // + if (cornerDiff < 0) //½ + { + if (pre_data->corner > 0) + cornerPeakP.push_back(*pre_data); + _state = 2; + } + break; + case 2: //½ + if (cornerDiff > 0) // + { + if (pre_data->corner < 0) + cornerPeakM.push_back(*pre_data); + _state = 1; + } + break; + default: + _state = 0; + break; + } + pre_data = curr_data; + pre_i = i; + } + //ע⣺һΪλ + + //Сֵ㣨嶥 + //ֵȽϣڳ߶ȴѰҾֲֵ + double square_distTh = 4 * cornerPara.scale * cornerPara.scale; //2cornerScale + for (int i = 0, i_max = (int)cornerPeakM.size(); i < i_max; i++) + { + if (abs(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 (abs(cornerPeakM[i].corner) < abs(cornerPeakM[j].corner)) + { + isPeak = false; + break; + } + } + // + if (true == isPeak) + { + cornerPtIdx = cornerPeakM[i].pntIdx; + for (int j = i + 1; j < i_max; j++) + { + int postPtIdx = cornerPeakM[j].pntIdx; + double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2); + if (dist > square_distTh) //߶ȴ + break; + + if (abs(cornerPeakM[i].corner) < abs(cornerPeakM[j].corner)) + { + isPeak = false; + break; + } + } + } + if (true == isPeak) + { + //жǷ䵽ˮƽƽ + SSG_basicFeature1D a_feature; + memset(&a_feature, 0, sizeof(SSG_basicFeature1D)); + a_feature.featureType = 0; + if (abs(cornerPeakM[i].backwardAngle) < cornerPara.jumpCornerTh_1) + a_feature.featureType = LINE_FEATURE_L_JUMP_H2L; + else if (abs(cornerPeakM[i].forwardAngle) < cornerPara.jumpCornerTh_1) + a_feature.featureType = LINE_FEATURE_L_JUMP_L2H; + if (a_feature.featureType > 0) + { + int idx = cornerPtIdx; //vldPts[cornerPtIdx].nPointIdx; + //MaskѰGroundԵ + bool vldContour = false; + int stepping = 1; + bool stop1 = false; + bool stop2 = false; + while (1) + { + if (false == stop1) + { + int idx_1 = idx + stepping; + if (idx_1 < (int)vldPts.size()) + { + double dist = sqrt(pow(vldPts[idx].pt3D.x - vldPts[idx_1].pt3D.x, 2) + + pow(vldPts[idx].pt3D.y - vldPts[idx_1].pt3D.y, 2)); + int raw_idx = vldPts[idx_1].nPointIdx; + if (dist > contourWin) + stop1 = true; + else if (1 == groundMask[raw_idx]) + vldContour = true; + } + else + stop1 = true; + } + if (false == stop2) + { + int idx_2 = idx - stepping; + if (idx_2 >= 0) + { + double dist = sqrt(pow(vldPts[idx].pt3D.x - vldPts[idx_2].pt3D.x, 2) + + pow(vldPts[idx].pt3D.y - vldPts[idx_2].pt3D.y, 2)); + int raw_idx = vldPts[idx_2].nPointIdx; + if (dist > contourWin) + stop2 = true; + else if (1 == groundMask[raw_idx]) + vldContour = true; + } + else + stop2 = true; + } + if ((true == vldContour) || ((true == stop1) && (true == stop2))) + break; + stepping++; + } + if (true == vldContour) + { + int raw_idx = vldPts[idx].nPointIdx; + a_feature.jumpPos = lineData[raw_idx].pt3D; + a_feature.jumpPos2D = { lineIdx, raw_idx }; + a_feature.featureValue = cornerPeakM[i].corner; + line_features.push_back(a_feature); + } + } + } + } + return; +} + +//ȡGapGapCornerɵ +void sg_getLineGapFeature( + std::vector& lineData, //ɨ + int lineIdx, //ǰɨ + const SSG_cornerParam cornerPara, + const SVzNLRangeD gapParam, + std::vector& line_gaps) +{ + int dataSize = (int)lineData.size(); + //ȥ + std::vector< SVzNL3DPosition> vldPts; + //std::vector< int> vldPtSegIdx; + std::vector segs; + std::vector backIndexing; + backIndexing.resize(dataSize); + + int runIdx = 1; + SSG_RUN_EX a_run = { 0, -1, 0, false, false }; //startIdx, len, lastIdx + double pre_z = 0; + double pre_y = 0; + for (int i = 0; i < dataSize; i++) + { + lineData[i].nPointIdx = i; //± + if (lineData[i].pt3D.z > 1e-4) + { + int bIdx = (int)vldPts.size(); + backIndexing[i] = bIdx; + vldPts.push_back(lineData[i]); + + if (a_run.len < 0) + { + a_run.start_zRising = true; + a_run.start = bIdx; + a_run.len = 1; + a_run.end_zRising = true; + a_run.value = i; + } + else + { + double z_diff = abs(lineData[i].pt3D.z - pre_z); + if (z_diff < cornerPara.minEndingGap_z) + { + a_run.len = bIdx - a_run.start + 1; + a_run.value = i; + } + else + { + bool next_zRising; + if (pre_z > lineData[i].pt3D.z) + { + a_run.end_zRising = true; + next_zRising = false; + } + else + { + a_run.end_zRising = false; + next_zRising = true; + } + a_run.value = runIdx; + runIdx++; + segs.push_back(a_run); + + a_run.start = bIdx; + a_run.start_zRising = next_zRising; + a_run.len = 1; + a_run.value = i; + a_run.end_zRising = true; + } + } + //vldPtSegIdx.push_back(runIdx); + + pre_z = lineData[i].pt3D.z; + } + } + if (a_run.len > 0) + segs.push_back(a_run); + + //ǰǺͺ + std::vector< SSG_pntDirAngle> corners; + corners.resize(vldPts.size()); + //ν + int segSize = (int)segs.size(); + for (int segIdx = 0; segIdx < segSize; segIdx++) + { + int vPtIdxStart = segs[segIdx].start; + int vPtIdxEnd = vPtIdxStart + segs[segIdx].len - 1; + for (int i = vPtIdxStart; i <= vPtIdxEnd; i++) + { + //ǰѰ + int pre_i = -1; + for (int j = i - 1; j >= vPtIdxStart; j--) + { + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if (dist >= cornerPara.scale) + { + pre_i = j; + break; + } + } + //Ѱ + int post_i = -1; + for (int j = i + 1; j <= vPtIdxEnd; j++) + { + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if (dist >= cornerPara.scale) + { + post_i = j; + break; + } + } + //ս + if ((pre_i < 0) || (post_i < 0)) + { + corners[i].pntIdx = -1; + corners[i].forwardAngle = 0; + corners[i].backwardAngle = 0; + corners[i].corner = 0; + corners[i].forwardDiffZ = 0; + corners[i].backwardDiffZ = 0; + } + else + { + double tanValue_pre = (vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z) / abs(vldPts[i].pt3D.y - vldPts[pre_i].pt3D.y); + double tanValue_post = (vldPts[post_i].pt3D.z - vldPts[i].pt3D.z) / abs(vldPts[post_i].pt3D.y - vldPts[i].pt3D.y); + double forwardAngle = atan(tanValue_post) * 180.0 / PI; + double backwardAngle = atan(tanValue_pre) * 180.0 / PI; + corners[i].pntIdx = i; + corners[i].forwardAngle = forwardAngle; + corners[i].backwardAngle = backwardAngle; + corners[i].corner = -(forwardAngle - backwardAngle); //ͼϵϵy෴С- + corners[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z; + corners[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z; + } + } + } + + //սǼֵ + int _state = 0; + int pre_i = -1; + int sEdgePtIdx = -1; + int eEdgePtIdx = -1; + SSG_pntDirAngle* pre_data = NULL; + std::vector< SSG_pntDirAngle> cornerPeakP; + std::vector< SSG_pntDirAngle> cornerPeakM; + for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) + { + if (i == 275) + int kkk = 1; + SSG_pntDirAngle* curr_data = &corners[i]; + if (curr_data->pntIdx < 0) + { + if (i == i_max - 1) //һ + { + if (1 == _state) // + { + cornerPeakP.push_back(corners[eEdgePtIdx]); + } + else if (2 == _state) //½ + { + cornerPeakM.push_back(corners[eEdgePtIdx]); + } + } + continue; + } + + if (NULL == pre_data) + { + sEdgePtIdx = i; + eEdgePtIdx = i; + pre_data = curr_data; + pre_i = i; + continue; + } + + eEdgePtIdx = i; + double cornerDiff = curr_data->corner - pre_data->corner; + switch (_state) + { + case 0: //̬ + if (cornerDiff < 0) //½ + { + _state = 2; + } + else if (cornerDiff > 0) // + { + _state = 1; + } + break; + case 1: // + if (cornerDiff < 0) //½ + { + if (pre_data->corner > 0) + cornerPeakP.push_back(*pre_data); + _state = 2; + } + break; + case 2: //½ + if (cornerDiff > 0) // + { + if (pre_data->corner < 0) + cornerPeakM.push_back(*pre_data); + _state = 1; + } + break; + default: + _state = 0; + break; + } + pre_data = curr_data; + pre_i = i; + } + //ϲ + //ֵȽϣڳ߶ȴѰҾֲֵ + std::vector< SSG_pntDirAngle> vldCornerPeaks; + double square_distTh = cornerPara.scale * cornerPara.scale; //2cornerScale + for (int i = 0, i_max = (int)cornerPeakM.size(); i < i_max; i++) + { + if (abs(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 (abs(cornerPeakM[i].corner) < abs(cornerPeakM[j].corner)) + { + isPeak = false; + break; + } + } + // + if (true == isPeak) + { + cornerPtIdx = cornerPeakM[i].pntIdx; + for (int j = i + 1; j < i_max; j++) + { + int postPtIdx = cornerPeakM[j].pntIdx; + double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2); + if (dist > square_distTh) //߶ȴ + break; + + if (abs(cornerPeakM[i].corner) < abs(cornerPeakM[j].corner)) + { + isPeak = false; + break; + } + } + } + if (true == isPeak) + { + vldCornerPeaks.push_back(cornerPeakM[i]); + } + } + + // + for (int i = 0, i_max = (int)vldCornerPeaks.size(); i < i_max-1; i++) + { + SSG_pntDirAngle& corner_1 = vldCornerPeaks[i]; + int ptIdx_1 = corner_1.pntIdx; + if (corner_1.type < 0) + continue; + + SSG_pntDirAngle& corner_2 = vldCornerPeaks[i + 1]; + int ptIdx_2 = corner_2.pntIdx; + double dist = sqrt(pow(vldPts[ptIdx_1].pt3D.y - vldPts[ptIdx_2].pt3D.y, 2) +pow(vldPts[ptIdx_1].pt3D.x - vldPts[ptIdx_2].pt3D.x, 2)); + if ((dist > gapParam.min) && (dist < gapParam.max)) + { + SSG_basicFeatureGap a_gap; + memset(&a_gap, 0, sizeof(SSG_basicFeatureGap)); + a_gap.gapPt_0 = vldPts[ptIdx_1]; + a_gap.gapPt_1 = vldPts[ptIdx_2]; + a_gap.lineIdx = lineIdx; + line_gaps.push_back(a_gap); + corner_2.type = -1; + } + } + return; +} + +/// +/// ȡϵĹյˮƽ˵ΪCorner +/// seg˵㣺zy +/// Segcornerȡ +/// nPointIdx¶Feature +/// 㷨̣ +/// 1ǰǺͺ +/// 2սǣ˳ʱΪʱΪ +/// 3սǵļֵ +/// 4жϹսǷΪ +/// +void sg_getLineContour_BQ( + SVzNL3DPosition* lineData, + int dataSize, + int lineIdx, + double refSteppingZ, + const SSG_cornerParam cornerPara, //scaleͨȡbagH1/4 + std::vector& line_features) +{ + //ȥ + std::vector< SVzNL3DPosition> vldPts; + std::vector< int> vldPtSegIdx; + std::vector segs; + std::vector backIndexing; + backIndexing.resize(dataSize); + + int runIdx = 1; + SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx + double pre_z = 0; + double pre_y = 0; + for (int i = 0; i < dataSize; i++) + { + if (i == 370) + int kkk = 1; + if (lineData[i].pt3D.z > 1e-4) + { + if (a_run.len < 0) + { + a_run.start = i; + a_run.len = 1; + a_run.value = i; + } + else + { + double z_diff = abs(lineData[i].pt3D.z - pre_z); + double y_diff = abs(lineData[i].pt3D.y - pre_y); + if ((z_diff < cornerPara.minEndingGap_z) && (y_diff < cornerPara.minEndingGap)) + { + a_run.len = i - a_run.start + 1; + a_run.value = i; + } + else + { + a_run.value = runIdx; + runIdx++; + segs.push_back(a_run); + + a_run.start = i; + a_run.len = 1; + a_run.value = i; + } + } + int bIdx = (int)vldPts.size(); + backIndexing[i] = bIdx; + vldPts.push_back(lineData[i]); + vldPtSegIdx.push_back(runIdx); + + pre_z = lineData[i].pt3D.z; + pre_y = lineData[i].pt3D.y; + } + } + if (a_run.len > 0) + segs.push_back(a_run); + + //ñ־ + for (int i = 0, i_max = (int)segs.size(); i < i_max; i++) + { + int idx1 = segs[i].start; + int idx2 = segs[i].start + segs[i].len - 1; + lineData[idx1].nPointIdx |= 0x100000; + lineData[idx2].nPointIdx |= 0x200000; + } + + //ǰǺͺ + std::vector< SSG_pntDirAngle> corners; + corners.resize(vldPts.size()); + for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) + { + if ((lineIdx == 399) && (i == 419)) + int kkk = 1; + //ǰѰ + int pre_i = -1; + for (int j = i - 1; j >= 0; j--) + { + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if ((dist >= cornerPara.scale) && (vldPtSegIdx[i] == vldPtSegIdx[j])) + { + pre_i = j; + break; + } + } + //Ѱ + int post_i = -1; + for (int j = i + 1; j < i_max; j++) + { + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if ((dist >= cornerPara.scale) && (vldPtSegIdx[i] == vldPtSegIdx[j])) + { + post_i = j; + break; + } + } + //ս + double tanValue_pre = 0; + if (pre_i >= 0) + tanValue_pre = (vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z) / abs(vldPts[i].pt3D.y - vldPts[pre_i].pt3D.y); + double tanValue_post = 0; + if (post_i >= 0) + tanValue_post = (vldPts[post_i].pt3D.z - vldPts[i].pt3D.z) / abs(vldPts[post_i].pt3D.y - vldPts[i].pt3D.y); + double forwardAngle = atan(tanValue_post) * 180.0 / PI; + double backwardAngle = atan(tanValue_pre) * 180.0 / PI; + + if ((pre_i < 0) && (post_i < 0)) + { + corners[i].pntIdx = -1; + corners[i].forwardAngle = 0; + corners[i].backwardAngle = 0; + corners[i].corner = 0; + corners[i].forwardDiffZ = 0; + corners[i].backwardDiffZ = 0; + } + else + { + corners[i].pntIdx = i; + corners[i].forwardAngle = forwardAngle; + corners[i].backwardAngle = backwardAngle; + if ((pre_i >= 0) && (post_i >= 0)) + { + corners[i].corner = -(forwardAngle - backwardAngle); //ͼϵϵy෴С- + corners[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z; + corners[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z; + } + else + { + corners[i].corner = 0; + corners[i].forwardDiffZ = 0; + corners[i].backwardDiffZ = 0; + } + } + } + + //սǼֵ + int _state = 0; + int pre_i = -1; + int sEdgePtIdx = -1; + int eEdgePtIdx = -1; + SSG_pntDirAngle* pre_data = NULL; + std::vector< SSG_pntDirAngle> cornerPeakP; + std::vector< SSG_pntDirAngle> cornerPeakM; + for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) + { + if (i == 275) + int kkk = 1; + SSG_pntDirAngle* curr_data = &corners[i]; + if (curr_data->pntIdx < 0) + { + if (i == i_max - 1) //һ + { + if (1 == _state) // + { + cornerPeakP.push_back(corners[eEdgePtIdx]); + } + else if (2 == _state) //½ + { + cornerPeakM.push_back(corners[eEdgePtIdx]); + } + } + continue; + } + + if (NULL == pre_data) + { + sEdgePtIdx = i; + eEdgePtIdx = i; + pre_data = curr_data; + pre_i = i; + continue; + } + + eEdgePtIdx = i; + double cornerDiff = curr_data->corner - pre_data->corner; + switch (_state) + { + case 0: //̬ + if (cornerDiff < 0) //½ + { + _state = 2; + } + else if (cornerDiff > 0) // + { + _state = 1; + } + break; + case 1: // + if (cornerDiff < 0) //½ + { + cornerPeakP.push_back(*pre_data); + _state = 2; + } + break; + case 2: //½ + if (cornerDiff > 0) // + { + cornerPeakM.push_back(*pre_data); + _state = 1; + } + break; + default: + _state = 0; + break; + } + pre_data = curr_data; + pre_i = i; + } + //ע⣺һΪλ + + //Сֵ㣨嶥 + //ֵȽϣڳ߶ȴѰҾֲֵ + double square_distTh = 4 * cornerPara.scale * cornerPara.scale; //2cornerScale + 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; + a_feature.featureType = 0; + if (abs(cornerPeakP[i].backwardAngle) < cornerPara.jumpCornerTh_1) + a_feature.featureType = LINE_FEATURE_L_JUMP_H2L; + else if (abs(cornerPeakP[i].forwardAngle) < cornerPara.jumpCornerTh_1) + a_feature.featureType = LINE_FEATURE_L_JUMP_L2H; + if (a_feature.featureType > 0) + { + int idx = vldPts[cornerPtIdx].nPointIdx; + lineData[idx].nPointIdx |= (a_feature.featureType << 16); + } + } + } + //Feature + for (int i = 0; i < dataSize; i++) + { + SVzNL3DPosition a_pt = lineData[i]; + int flag = a_pt.nPointIdx >> 16; + int endFlag = flag >> 4; + flag &= 0x0f; + + if (flag > 0) + { + double diffZ = abs(a_pt.pt3D.z - refSteppingZ); + if (diffZ < 10.0) + { + SSG_basicFeature1D a_feature; + memset(&a_feature, 0, sizeof(SSG_basicFeature1D)); + a_feature.featureType = flag; + a_feature.jumpPos = a_pt.pt3D; + a_feature.jumpPos2D = { lineIdx, i }; + line_features.push_back(a_feature); + } + } + else if (1 == endFlag) + { + int cornerIdx = backIndexing[i]; + if (abs(corners[cornerIdx].forwardAngle) < cornerPara.jumpCornerTh_1) + { + //double diffZ = abs(a_pt.pt3D.z - refSteppingZ); + //if (diffZ < 10.0) + { + SSG_basicFeature1D a_feature; + memset(&a_feature, 0, sizeof(SSG_basicFeature1D)); + a_feature.featureType = LINE_FEATURE_L_JUMP_L2H; + a_feature.jumpPos = a_pt.pt3D; + a_feature.jumpPos2D = { lineIdx, i }; + line_features.push_back(a_feature); + } + } + } + else if (2 == endFlag) + { + int cornerIdx = backIndexing[i]; + if (abs(corners[cornerIdx].backwardAngle) < cornerPara.jumpCornerTh_1) + { + //double diffZ = abs(a_pt.pt3D.z - refSteppingZ); + //if (diffZ < 10.0) + { + SSG_basicFeature1D a_feature; + memset(&a_feature, 0, sizeof(SSG_basicFeature1D)); + a_feature.featureType = LINE_FEATURE_L_JUMP_H2L; + a_feature.jumpPos = a_pt.pt3D; + a_feature.jumpPos2D = { lineIdx, i }; + line_features.push_back(a_feature); + } + } + } + } + return; +} + /// /// ȡϵ䡢zֵVLͣ⣨PSM) /// seg˵㣺z @@ -2484,7 +3450,7 @@ void wd_getRingArcFeature( int dataSize = (int)lineData.size(); //ȥ std::vector< SVzNL3DPosition> vldPts; - std::vector< int> vldPtSegIdx; + //std::vector< int> vldPtSegIdx; std::vector segs; std::vector backIndexing; backIndexing.resize(dataSize); @@ -2498,10 +3464,14 @@ void wd_getRingArcFeature( lineData[i].nPointIdx = i; //± if (lineData[i].pt3D.z > 1e-4) { + int bIdx = (int)vldPts.size(); + backIndexing[i] = bIdx; + vldPts.push_back(lineData[i]); + if (a_run.len < 0) { a_run.start_zRising = true; - a_run.start = i; + a_run.start = bIdx; a_run.len = 1; a_run.end_zRising = true; a_run.value = i; @@ -2511,7 +3481,7 @@ void wd_getRingArcFeature( double z_diff = abs(lineData[i].pt3D.z - pre_z); if (z_diff < cornerPara.minEndingGap_z) { - a_run.len = i - a_run.start + 1; + a_run.len = bIdx - a_run.start + 1; a_run.value = i; } else @@ -2531,17 +3501,14 @@ void wd_getRingArcFeature( runIdx++; segs.push_back(a_run); - a_run.start = i; + a_run.start = bIdx; a_run.start_zRising = next_zRising; a_run.len = 1; a_run.value = i; a_run.end_zRising = true; } } - int bIdx = (int)vldPts.size(); - backIndexing[i] = bIdx; - vldPts.push_back(lineData[i]); - vldPtSegIdx.push_back(runIdx); + //vldPtSegIdx.push_back(runIdx); pre_z = lineData[i].pt3D.z; } @@ -2563,54 +3530,61 @@ void wd_getRingArcFeature( //ǰǺͺ std::vector< SSG_pntDirAngle> corners; corners.resize(vldPts.size()); - for (int i = 0, i_max = (int)vldPts.size(); i < i_max; i++) + //ν + int segSize = (int)segs.size(); + for (int segIdx = 0; segIdx < segSize; segIdx++) { - //ǰѰ - int pre_i = -1; - for (int j = i - 1; j >= 0; j--) + int vPtIdxStart = segs[segIdx].start; + int vPtIdxEnd = vPtIdxStart + segs[segIdx].len - 1; + for (int i = vPtIdxStart; i <= vPtIdxEnd; i++) { - double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + - pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); - if (dist >= cornerPara.scale) + //ǰѰ + int pre_i = -1; + for (int j = i - 1; j >= vPtIdxStart; j--) { - pre_i = j; - break; + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if (dist >= cornerPara.scale) + { + pre_i = j; + break; + } } - } - //Ѱ - int post_i = -1; - for (int j = i + 1; j < i_max; j++) - { - double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + - pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); - if (dist >= cornerPara.scale) + //Ѱ + int post_i = -1; + for (int j = i + 1; j <= vPtIdxEnd; j++) { - post_i = j; - break; + double dist = sqrt(pow(vldPts[i].pt3D.y - vldPts[j].pt3D.y, 2) + + pow(vldPts[i].pt3D.z - vldPts[j].pt3D.z, 2)); + if (dist >= cornerPara.scale) + { + post_i = j; + break; + } + } + //ս + if ((pre_i < 0) || (post_i < 0)) + { + corners[i].pntIdx = -1; + corners[i].forwardAngle = 0; + corners[i].backwardAngle = 0; + corners[i].corner = 0; + corners[i].forwardDiffZ = 0; + corners[i].backwardDiffZ = 0; + } + else + { + double tanValue_pre = (vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z) / abs(vldPts[i].pt3D.y - vldPts[pre_i].pt3D.y); + double tanValue_post = (vldPts[post_i].pt3D.z - vldPts[i].pt3D.z) / abs(vldPts[post_i].pt3D.y - vldPts[i].pt3D.y); + double forwardAngle = atan(tanValue_post) * 180.0 / PI; + double backwardAngle = atan(tanValue_pre) * 180.0 / PI; + corners[i].pntIdx = i; + corners[i].forwardAngle = forwardAngle; + corners[i].backwardAngle = backwardAngle; + corners[i].corner = -(forwardAngle - backwardAngle); //ͼϵϵy෴С- + corners[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z; + corners[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z; } - } - //ս - if ((pre_i < 0) || (post_i < 0)) - { - corners[i].pntIdx = -1; - corners[i].forwardAngle = 0; - corners[i].backwardAngle = 0; - corners[i].corner = 0; - corners[i].forwardDiffZ = 0; - corners[i].backwardDiffZ = 0; - } - else - { - double tanValue_pre = (vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z) / abs(vldPts[i].pt3D.y - vldPts[pre_i].pt3D.y); - double tanValue_post = (vldPts[post_i].pt3D.z - vldPts[i].pt3D.z) / abs(vldPts[post_i].pt3D.y - vldPts[i].pt3D.y); - double forwardAngle = atan(tanValue_post) * 180.0 / PI; - double backwardAngle = atan(tanValue_pre) * 180.0 / PI; - corners[i].pntIdx = i; - corners[i].forwardAngle = forwardAngle; - corners[i].backwardAngle = backwardAngle; - corners[i].corner = -(forwardAngle - backwardAngle); //ͼϵϵy෴С- - corners[i].forwardDiffZ = vldPts[post_i].pt3D.z - vldPts[i].pt3D.z; - corners[i].backwardDiffZ = vldPts[i].pt3D.z - vldPts[pre_i].pt3D.z; } } @@ -2669,14 +3643,16 @@ void wd_getRingArcFeature( case 1: // if (cornerDiff < 0) //½ { - cornerPeakP.push_back(*pre_data); + if(pre_data->corner > 0) + cornerPeakP.push_back(*pre_data); _state = 2; } break; case 2: //½ if (cornerDiff > 0) // { - cornerPeakM.push_back(*pre_data); + if(pre_data->corner < 0) + cornerPeakM.push_back(*pre_data); _state = 1; } break; @@ -2691,7 +3667,7 @@ void wd_getRingArcFeature( std::vector lineFeatures; //Сֵ㣨嶥 //ֵȽϣڳ߶ȴѰҾֲֵ - double square_distTh = 4 * cornerPara.scale * cornerPara.scale; //2cornerScale + double square_distTh = 16 * cornerPara.scale * cornerPara.scale; //2cornerScale for (int i = 0, i_max = (int)cornerPeakP.size(); i < i_max; i++) { if (cornerPeakP[i].corner < cornerPara.cornerTh) @@ -2736,14 +3712,72 @@ void wd_getRingArcFeature( SSG_basicFeature1D a_feature; - if ((cornerPeakP[i].backwardAngle > cornerPara.jumpCornerTh_1) && (cornerPeakP[i].forwardAngle > -cornerPara.jumpCornerTh_2)) - a_feature.featureType = LINE_FEATURE_L_JUMP_H2L; - else if ((cornerPeakP[i].forwardAngle < -cornerPara.jumpCornerTh_1) && (cornerPeakP[i].backwardAngle < cornerPara.jumpCornerTh_2)) + if ((abs(cornerPeakP[i].backwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakP[i].forwardAngle) > cornerPara.jumpCornerTh_2)) a_feature.featureType = LINE_FEATURE_L_JUMP_L2H; + else if ((abs(cornerPeakP[i].forwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakP[i].backwardAngle) > cornerPara.jumpCornerTh_2)) + a_feature.featureType = LINE_FEATURE_L_JUMP_H2L; else a_feature.featureType = LINE_FEATURE_CORNER_V; + a_feature.featureValue = cornerPeakP[i].corner; + a_feature.jumpPos = vldPts[cornerPtIdx].pt3D; + a_feature.jumpPos2D = { 0, vldPts[cornerPtIdx].nPointIdx }; + lineFeatures.push_back(a_feature); + } + } + for (int i = 0, i_max = (int)cornerPeakM.size(); i < i_max; i++) + { + if (cornerPeakM[i].corner > -cornerPara.cornerTh) + continue; + + bool isPeak = true; + //ǰ + int cornerPtIdx = cornerPeakM[i].pntIdx; + for (int j = i - 1; j >= 0; j--) + { + int prePtIdx = cornerPeakM[j].pntIdx; + double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[prePtIdx].pt3D.y, 2); // + pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2) ; + if (dist > square_distTh) //߶ȴ + break; + + if (cornerPeakM[i].corner > cornerPeakM[j].corner) + { + isPeak = false; + break; + } + } + // + if (true == isPeak) + { + cornerPtIdx = cornerPeakM[i].pntIdx; + for (int j = i + 1; j < i_max; j++) + { + int postPtIdx = cornerPeakM[j].pntIdx; + double dist = pow(vldPts[cornerPtIdx].pt3D.y - vldPts[postPtIdx].pt3D.y, 2); // +pow(pkTop[i].pt3D.x - pkTop[j].pt3D.x, 2); + if (dist > square_distTh) //߶ȴ + break; + + if (cornerPeakM[i].corner > cornerPeakM[j].corner) + { + isPeak = false; + break; + } + } + } + if (true == isPeak) + { + + + SSG_basicFeature1D a_feature; + if ((abs(cornerPeakM[i].backwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakM[i].forwardAngle) > cornerPara.jumpCornerTh_2)) + a_feature.featureType = LINE_FEATURE_L_JUMP_L2H; + else if ((abs(cornerPeakM[i].forwardAngle) < cornerPara.jumpCornerTh_1) && (abs(cornerPeakM[i].backwardAngle) > cornerPara.jumpCornerTh_2)) + a_feature.featureType = LINE_FEATURE_L_JUMP_H2L; + else + a_feature.featureType = LINE_FEATURE_CORNER_V; + + a_feature.featureValue = cornerPeakM[i].corner; a_feature.jumpPos = vldPts[cornerPtIdx].pt3D; a_feature.jumpPos2D = { 0, vldPts[cornerPtIdx].nPointIdx }; lineFeatures.push_back(a_feature); @@ -2751,43 +3785,24 @@ void wd_getRingArcFeature( } //ӿʼͽ߽ - //segǷҪϲϲ - for (int i = 0, i_max = (int)segs.size(); i < i_max - 1; i++) - { - SSG_RUN_EX* nxt_seg = &segs[i + 1]; - SSG_RUN_EX* curr_seg = &segs[i]; - - int idx_1 = curr_seg->start + curr_seg->len - 1; - int idx_2 = nxt_seg->start; - double y_diff = abs(lineData[idx_1].pt3D.y - lineData[idx_2].pt3D.y); - double z_diff = abs(lineData[idx_1].pt3D.z - lineData[idx_2].pt3D.z); - if ((y_diff < cornerPara.minEndingGap) && (z_diff < cornerPara.minEndingGap_z)) //ϲ - { - int idx_end = nxt_seg->start + nxt_seg->len - 1; - nxt_seg->start = curr_seg->start; - nxt_seg->len = idx_end - curr_seg->start + 1; - curr_seg->value = 0; - } - } for (int i = 0, i_max = (int)segs.size(); i < i_max; i++) { - if (0 == segs[i].value) //ϲ - continue; - 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; - an_edge.jumpPos = lineData[idx_1].pt3D; - an_edge.jumpPos2D = { 0, idx_1 }; + int ptIdx = vldPts[idx_1].nPointIdx; + an_edge.jumpPos2D = { 0, ptIdx }; + an_edge.jumpPos = lineData[ptIdx].pt3D; lineFeatures.push_back(an_edge); - //line_features.insert(line_features.begin(), an_edge); //ͷ //β an_edge.featureType = LINE_FEATURE_LINE_ENDING_1; - an_edge.jumpPos = lineData[idx_2].pt3D; - an_edge.jumpPos2D = { 0, idx_2 }; + ptIdx = vldPts[idx_2].nPointIdx; + an_edge.jumpPos2D = { 0, ptIdx }; + an_edge.jumpPos = lineData[ptIdx].pt3D; lineFeatures.push_back(an_edge); } @@ -2803,19 +3818,20 @@ void wd_getRingArcFeature( 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_H2L)) + ((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) + else if ( (lineFeatures[i].featureType == LINE_FEATURE_L_JUMP_H2L) && (lineFeatures[i].featureValue <0)) { - if (lineFeatures[i + 1].featureType == LINE_FEATURE_LINE_ENDING_1) + 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.75; + double th1 = ringArcWidth * 0.5; double th2 = ringArcWidth * 1.25; if ((deltaY >= th1) && (deltaY < th2) && (deltaZ < ringArcWidth * 0.5)) { diff --git a/sourceCode/channelSpaceMeasure.cpp b/sourceCode/channelSpaceMeasure.cpp new file mode 100644 index 0000000..2ebddd3 --- /dev/null +++ b/sourceCode/channelSpaceMeasure.cpp @@ -0,0 +1,371 @@ +#include +#include "SG_baseDataType.h" +#include "SG_baseAlgo_Export.h" +#include "channelSpaceMeasure_Export.h" +#include +#include + +//version 1.0.0 : base version release to customer +std::string m_strVersion = "1.0.0"; +const char* wd_ChannelSPaceMeasureVersion(void) +{ + return m_strVersion.c_str(); +} + + +//ʱתʱ > 0 ˳ʱתʱ < 0 +SVzNL3DPoint _rotate2D(SVzNL3DPoint pt, double sinTheta, double cosTheta) +{ + SVzNL3DPoint rotatePt; + rotatePt.x = pt.x * cosTheta - pt.y * sinTheta; + rotatePt.y = pt.x * sinTheta + pt.y * cosTheta; + rotatePt.z = pt.z; + return rotatePt; +} + +void _XY_rotateLine(double angle, std::vector& line_src, std::vector& rotate_dst) +{ + rotate_dst.resize(line_src.size()); + double sinTheta = sin(PI * angle / 180); + double cosTheta = cos(PI * angle / 180); + for (int i = 0, i_max = (int)line_src.size(); i < i_max; i++) + { + rotate_dst[i] = line_src[i]; + rotate_dst[i].gapPt_0.pt3D = _rotate2D(line_src[i].gapPt_0.pt3D, sinTheta, cosTheta); + rotate_dst[i].gapPt_1.pt3D = _rotate2D(line_src[i].gapPt_1.pt3D, sinTheta, cosTheta); + } + return; +} + +double _computeChannelSpace( + std::vector& line_1, + std::vector& line_2) +{ + double meanSpace = 0; + int dataSize = (int)line_1.size(); + if (dataSize == 0) + return 0; + for (int i = 0; i < dataSize; i++) + { + double cy_1 = (line_1[i].gapPt_0.pt3D.y + line_1[i].gapPt_1.pt3D.y) / 2; + double cy_2 = (line_2[i].gapPt_0.pt3D.y + line_2[i].gapPt_1.pt3D.y) / 2; + meanSpace += abs(cy_2 - cy_1); + } + meanSpace = meanSpace / dataSize; + return meanSpace; +} + +double _computeGapMeanWidth(std::vector& gap_data) +{ + double meanWidth = 0; + int dataSize = (int)gap_data.size(); + if (dataSize == 0) + return 0; + for (int i = 0; i < dataSize; i++) + { + double width = abs(gap_data[i].gapPt_0.pt3D.y - gap_data[i].gapPt_1.pt3D.y); + meanWidth += width; + } + meanWidth = meanWidth / dataSize; + return meanWidth; +} + +double _computeGapMeanDepth(std::vector& gap_data, std::vector< std::vector>& scanLines) +{ + double meanDepth = 0; + int dataSize = (int)gap_data.size(); + if (dataSize == 0) + return 0; + + for (int i = 0; i < dataSize; i++) + { + int ptIdx_1 = gap_data[i].gapPt_0.nPointIdx; + int ptIdx_2 = gap_data[i].gapPt_1.nPointIdx; + int line = gap_data[i].lineIdx; + double max_z = 0; + for (int j = ptIdx_1; j <= ptIdx_2; j++) + { + if (scanLines[line][j].pt3D.z > 1e-4) + { + if (max_z < scanLines[line][j].pt3D.z) + max_z = scanLines[line][j].pt3D.z; + } + } + double depth = max_z - (gap_data[i].gapPt_0.pt3D.z + gap_data[i].gapPt_1.pt3D.z) / 2; + meanDepth += depth; + } + meanDepth = meanDepth / dataSize; + return meanDepth; +} + +SSX_channelInfo sx_channelSpaceMeasure( + std::vector< std::vector>& scanLines, + bool isHorizonScan, //true:ƽв۵false:ߴֱ۵ + const SSG_cornerParam cornerPara, + const SSG_outlierFilterParam filterParam, + const SSG_treeGrowParam growParam, + const SSX_channelParam channelParam, + int* errCode) +{ + *errCode = 0; + SSX_channelInfo channelInfo; + memset(&channelInfo, 0, sizeof(SSX_channelInfo)); + int lineNum = (int)scanLines.size(); + if (lineNum == 0) + { + *errCode = SG_ERR_3D_DATA_NULL; + return channelInfo; + } + + 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 channelInfo; + } + + std::vector< std::vector> 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> gapFeatures; + for (int line = 0; line < lineNum; line++) + { + if (line == 44) + int kkk = 1; + std::vector& lineData = data_lines[line]; + + //˲˳쳣 + sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam); + std::vector line_gaps; + int dataSize = (int)lineData.size(); + //ȡGap + sg_getLineGapFeature( + lineData, //ɨ + line, //ǰɨ + cornerPara, + channelParam.channelWidthRng, + line_gaps); + gapFeatures.push_back(line_gaps); + } + // + std::vector growTrees; + for (int line = 0; line < lineNum; line++) + { + bool isLastLine = false; + if (line == lineNum - 1) + isLastLine = true; + std::vector& line_gapFeatures = gapFeatures[line]; + + sg_lineGapsGrowing( + line, + isLastLine, + line_gapFeatures, + growTrees, + growParam); + } + if (growTrees.size() != 2) + { + *errCode = SG_ERR_NOT_GRID_FORMAT; + return channelInfo; + } + + std::vector& nodes_1 = growTrees[0].treeNodes; + std::vector& nodes_2 = growTrees[1].treeNodes; +#if _OUTPUT_DEBUG_DATA + for (int i = 0, i_max = (int)nodes_1.size(); i < i_max; i++) + { + int lineIdx, ptIdx; + if (false == isHorizonScan) + { + lineIdx = nodes_1[i].lineIdx; + ptIdx = nodes_1[i].gapPt_0.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 1; + ptIdx = nodes_1[i].gapPt_1.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 2; + } + else + { + ptIdx = nodes_1[i].lineIdx; + lineIdx = nodes_1[i].gapPt_0.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 1; + lineIdx = nodes_1[i].gapPt_1.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 2; + } + } + for (int i = 0, i_max = (int)nodes_2.size(); i < i_max; i++) + { + int lineIdx, ptIdx; + if (false == isHorizonScan) + { + lineIdx = nodes_2[i].lineIdx; + ptIdx = nodes_2[i].gapPt_0.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 1; + ptIdx = nodes_2[i].gapPt_1.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 2; + } + else + { + ptIdx = nodes_2[i].lineIdx; + lineIdx = nodes_2[i].gapPt_0.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 1; + lineIdx = nodes_2[i].gapPt_1.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 2; + } + } +#endif + + //ɨ + std::vector line_1; //line_1line_2Ϊɨ߶Ӧ۵ϵGap + std::vector line_2; + int i_idx = 0; + int j_idx = 0; + while (1) + { + int line_idx1 = nodes_1[i_idx].lineIdx; + int line_idx2 = nodes_2[j_idx].lineIdx; + if (line_idx1 == line_idx2) + { + line_1.push_back(nodes_1[i_idx]); + line_2.push_back(nodes_2[j_idx]); + i_idx++; + j_idx++; + } + else + { + if (line_idx1 < line_idx2) + i_idx++; + else + j_idx++; + } + if( (i_idx >= (int)nodes_1.size()) || (j_idx >= (int)nodes_2.size())) + break; + } + +#if _OUTPUT_DEBUG_DATA + for (int i = 0, i_max = (int)line_1.size(); i < i_max; i++) + { + int lineIdx, ptIdx; + if (false == isHorizonScan) + { + lineIdx = line_1[i].lineIdx; + ptIdx = line_1[i].gapPt_0.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 3; + ptIdx = line_1[i].gapPt_1.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 4; + } + else + { + ptIdx = line_1[i].lineIdx; + lineIdx = line_1[i].gapPt_0.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 3; + lineIdx = line_1[i].gapPt_1.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 4; + } + } + for (int i = 0, i_max = (int)line_2.size(); i < i_max; i++) + { + int lineIdx, ptIdx; + if (false == isHorizonScan) + { + lineIdx = line_2[i].lineIdx; + ptIdx = line_2[i].gapPt_0.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 3; + ptIdx = line_2[i].gapPt_1.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 4; + } + else + { + ptIdx = line_2[i].lineIdx; + lineIdx = line_2[i].gapPt_0.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 3; + lineIdx = line_2[i].gapPt_1.nPointIdx; + scanLines[lineIdx][ptIdx].nPointIdx = 4; + } + } +#endif + + //ת֤ɨGapֱ + double angleSearchWin = 30; + double angleStepping = 0.1; + + int loop = (int)(angleSearchWin / angleStepping); + double bestSpace = 0; + double rotateAngle = 0; + for (int i = -loop; i <= loop; i++) + { + double angle = i * angleStepping; + std::vector rotate_line_1; + _XY_rotateLine(angle, line_1, rotate_line_1); + std::vector rotate_line_2; + _XY_rotateLine(angle, line_2, rotate_line_2); + double meanSpace = _computeChannelSpace(rotate_line_1, rotate_line_2); + if (bestSpace < meanSpace) + { + bestSpace = meanSpace; + rotateAngle = angle; + } + } + std::vector calib_line_1; + _XY_rotateLine(rotateAngle, line_1, calib_line_1); + std::vector calib_line_2; + _XY_rotateLine(rotateAngle, line_2, calib_line_2); + + channelInfo.channelSpace = _computeChannelSpace(calib_line_1, calib_line_2); + channelInfo.channelWidth[0] = _computeGapMeanWidth(calib_line_1); + channelInfo.channelWidth[1] = _computeGapMeanWidth(calib_line_2); + channelInfo.channelDepth[0] = _computeGapMeanDepth(calib_line_1, data_lines); + channelInfo.channelDepth[1] = _computeGapMeanDepth(calib_line_2, data_lines); + return channelInfo; +} + + + + diff --git a/sourceCode/channelSpaceMeasure_Export.h b/sourceCode/channelSpaceMeasure_Export.h new file mode 100644 index 0000000..9b611f5 --- /dev/null +++ b/sourceCode/channelSpaceMeasure_Export.h @@ -0,0 +1,32 @@ +#pragma once + +#include "SG_algo_Export.h" +#include + +#define _OUTPUT_DEBUG_DATA 1 + +typedef struct +{ + SVzNLRangeD channelWidthRng; + SVzNLRangeD channleSpaceRng; +}SSX_channelParam; + +typedef struct +{ + double channelSpace; + double channelWidth[2]; + double channelDepth[2]; +}SSX_channelInfo; + +//汾 +SG_APISHARED_EXPORT const char* wd_ChannelSPaceMeasureVersion(void); + +//ȡ۵Ⱥͼ +SG_APISHARED_EXPORT SSX_channelInfo sx_channelSpaceMeasure( + std::vector< std::vector>& scanLines, + bool isHorizonScan, //true:ƽв۵false:ߴֱ۵ + const SSG_cornerParam cornerPara, + const SSG_outlierFilterParam filterParam, + const SSG_treeGrowParam growParam, + const SSX_channelParam channelParam, + int* errCode); diff --git a/sourceCode/gasFillingPortPosition.cpp b/sourceCode/gasFillingPortPosition.cpp index ba18718..b1ea33d 100644 --- a/sourceCode/gasFillingPortPosition.cpp +++ b/sourceCode/gasFillingPortPosition.cpp @@ -192,7 +192,7 @@ SSG_6DOF wd_getGasFillingPortPosition( SVzNL2DPoint a_seedPos = {x, y}; std::vector< SVzNL2DPoint> a_cluster; a_cluster.push_back(a_seedPos); - wd_pointClustering2D( + wd_gridPointClustering( featureInfoMask,//int¼ǺclusterIDһflag feature3DInfo,//double,¼Ϣ clusterCheckWin, // diff --git a/sourceCode/motorStatorPosition.cpp b/sourceCode/motorStatorPosition.cpp index 24cc86a..0d94317 100644 --- a/sourceCode/motorStatorPosition.cpp +++ b/sourceCode/motorStatorPosition.cpp @@ -33,6 +33,12 @@ typedef struct int neighborID[6]; }SSG_hexagonNeighbour; +typedef struct +{ + cv::RotatedRect fittingPara; + SVzNL3DPoint objCenter; +}SG_fittingInfo; + std::string m_strVersion = "1.0.0"; const char* wd_particleSegVersion(void) { @@ -353,11 +359,6 @@ bool compareByNeighbourNumber(const SSG_hexagonNeighbour& a, const SSG_hexagonNe { return a.linkNum < b.linkNum; } -typedef struct -{ - cv::RotatedRect fittingPara; - SVzNL3DPoint objCenter; -}SG_fittingInfo; //Բɨ趨ӻ //scanCenterΪɨģԲɨscanR1scanR2Բ @@ -602,7 +603,180 @@ bool computeOuterGraspPoint( return false; } +//и +void cloudPointsFilter_zRange(std::vector< std::vector>& scanLines, SVzNLRangeD zRange, std::vector< std::vector>& cutData) +{ + int lineNum = (int)scanLines.size(); + cutData.resize(lineNum); + for (int line = 0; line < lineNum; line++) + { + int ptNum = (int)scanLines[line].size(); + cutData[line].resize(ptNum); + for (int i = 0; i < ptNum; i++) + { + SVzNL3DPosition a_pt = scanLines[line][i]; + if ((a_pt.pt3D.z < zRange.min) || (a_pt.pt3D.z > zRange.max)) + a_pt.pt3D.z = 0; + cutData[line][i] = a_pt; + } + } +} + +//޵ +void genCLusterMask( + std::vector< std::vector>& clusterSrcData, + bool maskType_null_vldPt, //true: nullPt(z=0); false:valid Point + std::vector>& featureInfoMask, + std::vector>& feature3DInfo) +{ + int lineNum = (int)clusterSrcData.size(); + if (lineNum == 0) + return; + int linePtNum = (int)clusterSrcData[0].size(); + + featureInfoMask.resize(lineNum); + feature3DInfo.resize(lineNum); + for (int i = 0; i < lineNum; i++) + { + featureInfoMask[i].resize(linePtNum); + feature3DInfo[i].resize(linePtNum); + } + //Mask + for (int line = 0; line < lineNum; line++) + { + std::vector& lineData = clusterSrcData[line]; + for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++) + { + if( ((false == maskType_null_vldPt)&&(clusterSrcData[line][ptIdx].pt3D.z > 1e-4))|| + ((true == maskType_null_vldPt) && (clusterSrcData[line][ptIdx].pt3D.z < 1e-4))) + { + SSG_featureClusteringInfo a_mask; + memset(&a_mask, 0, sizeof(SSG_featureClusteringInfo)); + a_mask.featurType = 1; + a_mask.lineIdx = line; + a_mask.ptIdx = ptIdx; + featureInfoMask[line][ptIdx] = a_mask; + feature3DInfo[line][ptIdx] = clusterSrcData[line][ptIdx].pt3D; + } + } + } + return; +} + +//:Чľʵͨ +void cloutPointsClustering_nullPt( + std::vector< std::vector>& clusterSrcData, + std::vector>& featureInfoMask, + std::vector>& feature3DInfo, + std::vector>& clusters, //ֻ¼λ + std::vector& clustersInfo) +{ + int lineNum = (int)clusterSrcData.size(); + if (lineNum == 0) + return; + int linePtNum = (int)clusterSrcData[0].size(); + //õ˼룬ع˼·иЧ + int clusterID = 1; + int clusterCheckWin = 5; + for (int y = 0; y < linePtNum; y++) + { + for (int x = 0; x < lineNum; x++) + { + SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y]; + if ((0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //Ѿ + continue; + + SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y]; + SVzNL3DRangeD a_clusterRoi3D; + memset(&a_clusterRoi3D, 0, sizeof(SVzNL3DRangeD)); + SVzNLRect clusterRoi2D; + clusterRoi2D.left = x; + clusterRoi2D.right = x; + clusterRoi2D.top = y; + clusterRoi2D.bottom = y; + + SVzNL2DPoint a_seedPos = { x, y }; + std::vector< SVzNL2DPoint> a_cluster; + a_cluster.push_back(a_seedPos); + + //ʹþ෽8ͨͨ + wd_gridPointClustering_labelling( + featureInfoMask, + feature3DInfo, + clusterID, //ǰClusterID + a_cluster, //result + clusterRoi2D //roi2D + ); + clusters.push_back(a_cluster); + SWD_clustersInfo a_info; + a_info.clusterIdx = clusterID; + a_info.ptSize = (int)a_cluster.size(); + a_info.roi2D = clusterRoi2D; + a_info.roi3D = a_clusterRoi3D; + clustersInfo.push_back(a_info); + clusterID++; + } + } +} + +// +void cloutPointsClustering( + std::vector< std::vector>& clusterSrcData, + std::vector>& featureInfoMask, + std::vector>& feature3DInfo, + SSG_treeGrowParam growParam, + std::vector>& clusters, //ֻ¼λ + std::vector& clustersInfo) +{ + int lineNum = (int)clusterSrcData.size(); + if (lineNum == 0) + return; + int linePtNum = (int)clusterSrcData[0].size(); + //õ˼룬ع˼·иЧ + int clusterID = 1; + int clusterCheckWin = 5; + for (int y = 0; y < linePtNum; y++) + { + for (int x = 0; x < lineNum; x++) + { + SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y]; + if ((0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //Ѿ + continue; + + SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y]; + SVzNL3DRangeD a_clusterRoi; + a_clusterRoi.xRange.min = a_feature3DValue.x; + a_clusterRoi.xRange.max = a_feature3DValue.x; + a_clusterRoi.yRange.min = a_feature3DValue.y; + a_clusterRoi.yRange.max = a_feature3DValue.y; + a_clusterRoi.zRange.min = a_feature3DValue.z; + a_clusterRoi.zRange.max = a_feature3DValue.z; + + SVzNL2DPoint a_seedPos = { x, y }; + std::vector< SVzNL2DPoint> a_cluster; + a_cluster.push_back(a_seedPos); + wd_gridPointClustering( + featureInfoMask,//int¼ǺclusterIDһflag + feature3DInfo,//double,¼Ϣ + clusterCheckWin, // + growParam,// + clusterID, //ǰClusterID + a_cluster, //result + a_clusterRoi + ); + clusters.push_back(a_cluster); + SWD_clustersInfo a_info; + a_info.clusterIdx = clusterID; + a_info.ptSize = (int)a_cluster.size(); + a_info.roi3D = a_clusterRoi; + clustersInfo.push_back(a_info); + clusterID++; + } + } +} + //Ӷλ +//1Ŀ꣨2ץȡ //㷨߼ҵӵĸ߶->ýȡZȥ-> // ͶӰעʱ߿ҲͬʱͶӰ->任->ȡĿ-> // ںͱ߿ѰץȡĿץȡ @@ -614,8 +788,7 @@ void wd_motorStatorPosition( SWD_nextOpParam* refPos, //һθIJολãͬʱһεIJολ int* errCode, std::vector& resultObjPositions, - SWD_statorOuterGrasper& resultGraspPos -) + SWD_statorOuterGrasper& resultGraspPos) { int lineNum = (int)scanLines.size(); if (lineNum == 0) @@ -629,14 +802,22 @@ void wd_motorStatorPosition( return; ///ʼݴ + //ʹcutZ_levelи + + + double statorRingWidth = (statorParam.statorOuterD - statorParam.statorInnerD) / 2; + // + + + if (refPos->cuttingZ < 0) //ȡϰ벿ĻȡĻж϶Ӹ߶ { //ֱݴ std::vector> all_vLineArcs; for (int i = 0; i < lineNum; i++) { - if (i == 202) + if (i == 576) int k = 1; //ȡӻ std::vector line_ringArcs; diff --git a/sourceCode/motorStatorPosition_Export.h b/sourceCode/motorStatorPosition_Export.h index 5d9b3b4..c17c3e0 100644 --- a/sourceCode/motorStatorPosition_Export.h +++ b/sourceCode/motorStatorPosition_Export.h @@ -14,6 +14,8 @@ typedef struct double statorOuterD; //ֱ double statorInnerD; //ڿֱ double statorHeight; + double cutZ_level0; + double cutZ_level1; double gripperR;//צ뾶 }SWD_statorParam; @@ -46,13 +48,16 @@ SG_APISHARED_EXPORT void wd_lineDataR( double groundH); ///gridʽԽˮƽʹֱĴ -SG_APISHARED_EXPORT void wd_motorStatorPosition( +//Ӷλ +//㷨߼ҵӵĸ߶->ýȡZȥ-> +// ͶӰעʱ߿ҲͬʱͶӰ->任->ȡĿ-> +// ںͱ߿ѰץȡĿץȡ +SG_APISHARED_EXPORT void wd_motorStatorPosition( std::vector< std::vector>& scanLines, - const SWD_statorParam positionParam, + const SWD_statorParam statorParam, const SSG_planeCalibPara groundCalibPara, const SWD_statorPositonParam algoParam, SWD_nextOpParam* refPos, //һθIJολãͬʱһεIJολ int* errCode, std::vector& resultObjPositions, - SWD_statorOuterGrasper& resultGraspPos -); \ No newline at end of file + SWD_statorOuterGrasper& resultGraspPos); \ No newline at end of file diff --git a/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp b/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp index 4cb4b58..02ecfb7 100644 --- a/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp +++ b/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp @@ -2636,8 +2636,8 @@ void _outputScanDataFile_removeZeros(char* fileName, SVzNL3DLaserLine * scanData } #define TEST_CONVERT_TO_GRID 0 -#define TEST_COMPUTE_WHEEL_ARCH 1 -#define TEST_COMPUTE_CALIB_PARA 0 +#define TEST_COMPUTE_WHEEL_ARCH 0 +#define TEST_COMPUTE_CALIB_PARA 1 #define TEST_GROUP 1 int main() @@ -2683,7 +2683,7 @@ int main() #if TEST_COMPUTE_CALIB_PARA char _calib_datafile[256]; - sprintf_s(_calib_datafile, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/LaserLine1_grid.txt"); + sprintf_s(_calib_datafile, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/ground_LaserData.txt"); std::vector< std::vector> scanLines; vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanLines); @@ -2707,7 +2707,7 @@ int main() sprintf_s(calibFile, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/ground_calib_para.txt"); _outputCalibPara(calibFile, calibPara); char _out_file[256]; - sprintf_s(_out_file, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/LaserLine1_grid_calib.txt"); + sprintf_s(_out_file, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/LaserLine_grund_calib.txt"); _outputScanDataFile_self(_out_file, scanLines, 0, 0, 0); printf("%s: calib done!\n", _calib_datafile); } @@ -2809,7 +2809,7 @@ int main() growParam, poseCalibPara, &errCode); -#endif + long t2 = GetTickCount64(); char _dbg_file[256]; @@ -2823,6 +2823,7 @@ int main() } } +#endif printf("all done!\n"); }