diff --git a/SG_Algorithm.sln b/SG_Algorithm.sln index 0527c64..b41a680 100644 --- a/SG_Algorithm.sln +++ b/SG_Algorithm.sln @@ -66,6 +66,16 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "SieveNodeDetection_test", " {D47DE411-B18C-4A64-BDB9-0AFAB50E8A8A} = {D47DE411-B18C-4A64-BDB9-0AFAB50E8A8A} EndProjectSection EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "QRcode3Ddetection", "QRcode3Ddetection\QRcode3Ddetection.vcxproj", "{A7C72ED9-B81D-4C6A-BE38-D75199C824AA}" + ProjectSection(ProjectDependencies) = postProject + {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "QRcode3Ddetection_test", "QRcode3Ddetection_test\QRcode3Ddetection_test.vcxproj", "{E5FAAACF-703C-4460-93B8-301991F62778}" + ProjectSection(ProjectDependencies) = postProject + {A7C72ED9-B81D-4C6A-BE38-D75199C824AA} = {A7C72ED9-B81D-4C6A-BE38-D75199C824AA} + EndProjectSection +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|x64 = Debug|x64 @@ -178,6 +188,22 @@ Global {DF0881C7-43D0-4765-97CA-74C126665F3E}.Release|x64.Build.0 = Release|x64 {DF0881C7-43D0-4765-97CA-74C126665F3E}.Release|x86.ActiveCfg = Release|Win32 {DF0881C7-43D0-4765-97CA-74C126665F3E}.Release|x86.Build.0 = Release|Win32 + {A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Debug|x64.ActiveCfg = Debug|x64 + {A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Debug|x64.Build.0 = Debug|x64 + {A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Debug|x86.ActiveCfg = Debug|Win32 + {A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Debug|x86.Build.0 = Debug|Win32 + {A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Release|x64.ActiveCfg = Release|x64 + {A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Release|x64.Build.0 = Release|x64 + {A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Release|x86.ActiveCfg = Release|Win32 + {A7C72ED9-B81D-4C6A-BE38-D75199C824AA}.Release|x86.Build.0 = Release|Win32 + {E5FAAACF-703C-4460-93B8-301991F62778}.Debug|x64.ActiveCfg = Debug|x64 + {E5FAAACF-703C-4460-93B8-301991F62778}.Debug|x64.Build.0 = Debug|x64 + {E5FAAACF-703C-4460-93B8-301991F62778}.Debug|x86.ActiveCfg = Debug|Win32 + {E5FAAACF-703C-4460-93B8-301991F62778}.Debug|x86.Build.0 = Debug|Win32 + {E5FAAACF-703C-4460-93B8-301991F62778}.Release|x64.ActiveCfg = Release|x64 + {E5FAAACF-703C-4460-93B8-301991F62778}.Release|x64.Build.0 = Release|x64 + {E5FAAACF-703C-4460-93B8-301991F62778}.Release|x86.ActiveCfg = Release|Win32 + {E5FAAACF-703C-4460-93B8-301991F62778}.Release|x86.Build.0 = Release|Win32 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/SieveNodeDetection_test/SieveNodeDetection_test.cpp b/SieveNodeDetection_test/SieveNodeDetection_test.cpp index 29704d4..ddd7670 100644 --- a/SieveNodeDetection_test/SieveNodeDetection_test.cpp +++ b/SieveNodeDetection_test/SieveNodeDetection_test.cpp @@ -484,7 +484,7 @@ typedef struct int b; }SG_color; void _outputScanDataFile_RGBD_obj_XYZ(char* fileName, SVzNL3DLaserLine* scanData, int lineNum, - float lineV, int maxTimeStamp, int clockPerSecond, std::vector& objOps) + float lineV, int maxTimeStamp, int clockPerSecond, std::vector>& objOps) { std::ofstream sw(fileName); int realLines = lineNum; @@ -605,35 +605,46 @@ void _outputScanDataFile_RGBD_obj_XYZ(char* fileName, SVzNL3DLaserLine* scanData } if (objOps.size() > 0) { - int ptNum = objOps.size(); - sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << ptNum << std::endl; - for (int i = 0; i < objOps.size(); i++) + int objNum = 0; + for (int i = 0, i_max = objOps.size(); i < i_max; i++) { - if (i == 0) - rgb = { 255, 0, 0 }; - else - rgb = { 255, 255, 0 }; - size = 25; - float x = (float)objOps[i].x; - float y = (float)objOps[i].y; - float z = (float)objOps[i].z; + std::vector& obj_line = objOps[i]; + objNum += obj_line.size(); + } - sw << "{" << x << "," << y << "," << z << "}-"; - sw << "{0,0}-{0,0}-"; - sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; - if (i == 0) + sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << objNum << std::endl; + for (int i = 0, i_max = objOps.size(); i < i_max; i++) + { + std::vector& obj_line = objOps[i]; + for (int j = 0, j_max = obj_line.size(); j < j_max; j++) { + rgb = objColor[i%8]; + size = 25; + float x = (float)obj_line[j].x; + float y = (float)obj_line[j].y; + float z = (float)obj_line[j].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; + } } } } sw.close(); } -void _XOYprojection_nodePos(cv::Mat& img, std::vector>& dataLines, std::vector& objOps, - const double x_scale, const double y_scale, const SVzNLRangeD x_range, const SVzNLRangeD y_range) +void _XOYprojection_nodePos( + cv::Mat& img, + std::vector>& dataLines, + std::vector>& objOps, + const double x_scale, const double y_scale, + const SVzNLRangeD x_range, const SVzNLRangeD y_range) { int x_skip = 16; int y_skip = 16; @@ -748,27 +759,24 @@ void _XOYprojection_nodePos(cv::Mat& img, std::vector 0) { - for (int i = 0; i < objOps.size(); i++) + for (int i = 0, i_max = objOps.size(); i < i_max; i++) { - if (i == 0) + std::vector& obj_line = objOps[i]; + for (int j = 0, j_max = obj_line.size(); j < j_max; j++) { - rgb = { 255, 0, 0 }; + rgb = objColor[i % 8]; size = 20; + + int px = (int)((obj_line[j].x - x_range.min) / x_scale + x_skip); + int py = (int)((obj_line[j].y - y_range.min) / y_scale + y_skip); + cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); } - else - { - rgb = { 255, 255, 0 }; - size = 10; - } - int px = (int)((objOps[i].x - x_range.min) / x_scale + x_skip); - int py = (int)((objOps[i].y - y_range.min) / y_scale + y_skip); - cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); } } } -void _genXOYProjectionImage_nodePos(cv::String& fileName, SVzNL3DLaserLine* scanData, int lineNum, std::vector& nodeOps) +void _genXOYProjectionImage_nodePos(cv::String& fileName, SVzNL3DLaserLine* scanData, int lineNum, std::vector>& nodeOps) { //统计X和Y的范围 std::vector> scan_lines; @@ -911,12 +919,160 @@ SSG_planeCalibPara _readCalibPara(char* fileName) return planePara; } +std::vector _wdReadLMIData_TXT(char* fileName) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + std::vector resultData; + if (inputFile.is_open() == false) + return resultData; + + while (getline(inputFile, linedata)) + { + double x, y, z; + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &y, &x, &z); + SVzNL3DPoint a_pt; + a_pt.x = x; + a_pt.y = y; + a_pt.z = z; + resultData.push_back(a_pt); + } + return resultData; +} + +std::vector _wdReadGuangZiData_TXT(char* fileName, double tick_scale, double offset) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + std::vector resultData; + if (inputFile.is_open() == false) + return resultData; + + while (getline(inputFile, linedata)) + { + double x, y, z; + sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &x, &y, &z); + if (z > -9999.0 + 1e-4) //1e-4为防止float误差 + { + SVzNL3DPoint a_pt; + a_pt.x = (x - offset)* tick_scale; + a_pt.y = y; + a_pt.z = z; + resultData.push_back(a_pt); + } + } + return resultData; +} + +SVzNL3DLaserLine* _convertToVizumDataFormat(std::vector& ptList, int* vldLineNum) +{ + + std::vector> lineList; + std::vector< SVzNL3DPoint> a_line; + double pre_y = 1.0e+10;; + int maxLineSize = 0; + for (int i = 0, i_max = ptList.size(); i < i_max; i++) + { + if (ptList[i].y < pre_y-1.0) //1.0为容错门限，防止相邻点有反转 + { + //新行开始 + if (a_line.size() > 0) + { + lineList.push_back(a_line); + if (maxLineSize < a_line.size()) + maxLineSize = a_line.size(); + a_line.clear(); + } + a_line.push_back(ptList[i]); + } + else + a_line.push_back(ptList[i]); + pre_y = ptList[i].y; + } + //最后一行 + if (a_line.size() > 0) + { + lineList.push_back(a_line); + if (maxLineSize < a_line.size()) + maxLineSize = a_line.size(); + } + + int lineNum = lineList.size(); + *vldLineNum = lineNum; + SVzNL3DLaserLine* resultdData = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * lineNum); + memset(resultdData, 0, sizeof(SVzNL3DLaserLine) * lineNum); + for (int line = 0; line < lineNum; line++) + { + std::vector< SVzNL3DPoint>& a_lineData = lineList[line]; + int pt_counter = a_lineData.size(); + resultdData[line].nPositionCnt = pt_counter; + resultdData[line].nTimeStamp = 0; + resultdData[line].p3DPosition = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * pt_counter); + memset(resultdData[line].p3DPosition, 0, sizeof(SVzNL3DPosition) * pt_counter); + for (int i = 0; i < pt_counter; i++) + { + resultdData[line].p3DPosition[i].nPointIdx = i; + resultdData[line].p3DPosition[i].pt3D = a_lineData[i]; + } + } + return resultdData; +} + + #define TEST_CONVERT_TO_GRID 0 #define TEST_COMPUTE_SIEVE_NODES 1 #define TEST_COMPUTE_CALIB_PARA 0 +#define TEST_CONVERT_LMI_DATA 0 +#define TEST_CONVERT_GUANGZI_DATA 0 #define TEST_GROUP 2 int main() { +#if TEST_CONVERT_LMI_DATA + //将数据转换成栅格格式格式 + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + char _LMI_file[256]; + sprintf_s(_LMI_file, "F:\\张奔-标定采图\\钢块-标定采图\\5-右下-2.txt"); + std::vector LMI_data = _wdReadLMIData_TXT(_LMI_file); + if (LMI_data.size() == 0) + return 0; + + int vldLineNum = 0; + SVzNL3DLaserLine* convertData = _convertToVizumDataFormat(LMI_data, &vldLineNum); + char _out_file[256]; + sprintf_s(_out_file, "F:\\张奔-标定采图\\钢块-标定采图\\5-右下-2_onverted.txt"); + _outputScanDataFile_self(_out_file, convertData, vldLineNum, + lineV, maxTimeStamp, clockPerSecond); + printf("%s: convert done!\n", _LMI_file); +#endif +#if TEST_CONVERT_GUANGZI_DATA + //将数据转换成栅格格式格式 + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + char _Guangzi_file[256]; + double offset = 300000; + double tick_scale = 0.05; //左正 + sprintf_s(_Guangzi_file, "F:\\张奔-标定采图\\钢块-标定采图\\5-右正-2.txt"); + std::vector Guangzi_data = _wdReadGuangZiData_TXT(_Guangzi_file, tick_scale, offset); + if (Guangzi_data.size() == 0) + return 0; + + int vldLineNum = 0; + SVzNL3DLaserLine* convertData = _convertToVizumDataFormat(Guangzi_data, &vldLineNum); + char _out_file[256]; + sprintf_s(_out_file, "F:\\张奔-标定采图\\钢块-标定采图\\5-右正-2_onverted.txt"); + _outputScanDataFile_self(_out_file, convertData, vldLineNum, + lineV, maxTimeStamp, clockPerSecond); + printf("%s: convert done!\n", _Guangzi_file); +#endif #if TEST_CONVERT_TO_GRID //将数据转换成栅格格式格式 char _scan_dir[256]; @@ -1111,7 +1267,7 @@ int main() sg_lineDataR(&laser3DPoints[i], poseCalibPara.planeCalib, poseCalibPara.planeHeight); } //计算节点位置 - std::vector nodePos; + std::vector> nodePos; sg_getSieveNodes( laser3DPoints, lineNum, diff --git a/bagPositioning_test/bagPositioning_test.cpp b/bagPositioning_test/bagPositioning_test.cpp index 89ed7a7..5c77ba2 100644 --- a/bagPositioning_test/bagPositioning_test.cpp +++ b/bagPositioning_test/bagPositioning_test.cpp @@ -11,6 +11,17 @@ #include #include +typedef struct +{ + int nPointIdx; + double x; + double y; + double z; + float r; + float g; + float b; +} SPointXYZRGB; + SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9]) { SVzNL3DPoint _r_pt; @@ -18,7 +29,15 @@ SVzNL3DPoint _ptRotate(SVzNL3DPoint pt3D, double matrix3d[9]) _r_pt.y = pt3D.x * matrix3d[3] + pt3D.y * matrix3d[4] + pt3D.z * matrix3d[5]; _r_pt.z = pt3D.x * matrix3d[6] + pt3D.y * matrix3d[7] + pt3D.z * matrix3d[8]; return _r_pt; +} +SVzNLPointXYZRGBA _ptRotate_RGBD(SVzNLPointXYZRGBA pt3D, double matrix3d[9]) +{ + SVzNLPointXYZRGBA _r_pt; + _r_pt.x = pt3D.x * matrix3d[0] + pt3D.y * matrix3d[1] + pt3D.z * matrix3d[2]; + _r_pt.y = pt3D.x * matrix3d[3] + pt3D.y * matrix3d[4] + pt3D.z * matrix3d[5]; + _r_pt.z = pt3D.x * matrix3d[6] + pt3D.y * matrix3d[7] + pt3D.z * matrix3d[8]; + return _r_pt; } #define DATA_VER_OLD 0 @@ -134,15 +153,24 @@ SVzNLXYZRGBDLaserLine* vzReadLaserScanPointFromFile_XYZRGB(const char* fileName, int imageY = 0; float leftX, leftY; float rightX, rightY; - int r = -1, g = -1, b = -1; + float r, g, b; sscanf_s(linedata.c_str(), "{%f,%f,%f,%f,%f,%f }-{%f,%f}-{%f,%f}", &X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY); int id = _scanLines[lineIdx].nPointCnt; if (id < ptNum) { + if (lineIdx == 537) + int kkk = 1; p3DPoint[id].x = X; p3DPoint[id].y = Y; p3DPoint[id].z = Z; - p3DPoint[id].nRGB = 0; + int nr = (int)(r * 255); + int ng = (int)(g * 255); + int nb = (int)(b * 255); + nb <<= 8; + nb += ng; + nb <<= 8; + nb += nr; + p3DPoint[id].nRGB = nb; _scanLines[lineIdx].nPointCnt = id + 1; } } @@ -209,6 +237,41 @@ SVzNLXYZRGBDLaserLine* vzReadLaserScanPointFromFile_XYZRGB(const char* fileName, return _scanLines; } +void vzReadLaserScanPointFromFile_XYZRGB_vector(const char* fileName, std::vector>& scanData) +{ + std::ifstream inputFile(fileName); + std::string linedata; + + if (inputFile.is_open() == false) + return; + + std::vector< SPointXYZRGB> a_line; + while (getline(inputFile, linedata)) + { + if (0 == strncmp("Line_", linedata.c_str(), 5)) //new line + { + if (a_line.size() > 0) + { + scanData.push_back(a_line); + a_line.clear(); + } + } + else if (0 == strncmp("{", linedata.c_str(), 1)) + { + float leftX, leftY; + float rightX, rightY; + SPointXYZRGB a_pt; + sscanf_s(linedata.c_str(), "{ %lf, %lf, %lf, %f, %f, %f }-{ %f, %f }-{ %f, %f }", &a_pt.x, &a_pt.y, &a_pt.z, &a_pt.r, &a_pt.g, &a_pt.b, &leftX, &leftY, &rightX, &rightY); + a_line.push_back(a_pt); + } + } + if(a_line.size() > 0) + scanData.push_back(a_line); + + inputFile.close(); + return; +} + SVzNL3DLaserLine* vzReadLaserScanPointFromFile_XYZ(const char* fileName, int* scanLineNum, float* scanV, int* dataCalib, int* scanMaxStamp, int* canClockUnit) { @@ -453,6 +516,53 @@ SVzNL3DLaserLine* _convertToGridData_XYZRGB(SVzNLXYZRGBDLaserLine* laser3DPoints return gridData; } +void _convertToGridData_XYZRGB_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< SPointXYZRGB>& a_line = scanData[line]; + int nPointCnt = a_line.size(); + for (int i = 0; i < nPointCnt; i++) + { + SPointXYZRGB* a_pt = &scanData[line][i]; + if (a_pt->z > 1e-4) + { + double v = _F * a_pt->y / a_pt->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; + std::vector< SPointXYZRGB> gridData; + gridData.resize(pt_counter); + for (int i = 0; i < pt_counter; i++) + gridData[i] = { 0, 0.0, 0.0, 0.0, 0.0f, 0.0f, 0.0f }; + for (int line = 0; line < lineNum; line++) + { + std::vector< SPointXYZRGB>& a_line = scanData[line]; + int nPointCnt = a_line.size(); + for (int i = 0; i < nPointCnt; i++) + { + SPointXYZRGB a_pt = a_line[i]; + if (a_pt.z > 1e-4) + { + int pt_id = a_pt.nPointIdx - min_y; + gridData[pt_id]= a_pt; + } + } + scanData_grid.push_back(gridData); + } + return; +} + void _outputCalibPara(char* fileName, SSG_planeCalibPara calibPara) { std::ofstream sw(fileName); @@ -546,6 +656,36 @@ void _outputScanDataFile_self(char* fileName, SVzNL3DLaserLine* scanData, int li sw.close(); } +void _outputScanDataFile_SPointXYZRGB_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++) + { + SPointXYZRGB* pt3D = &scanData[line][i]; + float x = (float)pt3D->x; + float y = (float)pt3D->y; + float z = (float)pt3D->z; + char str[250]; + sprintf_s(str, "{ %lf, %lf, %lf, %f, %f, %f } - { 0, 0 } - { 0, 0 }", + pt3D->x, pt3D->y, pt3D->z, pt3D->r, pt3D->g, pt3D->b); + + sw << str << std::endl; + } + } + sw.close(); +} + typedef struct { int r; @@ -704,6 +844,307 @@ void _outputScanDataFile_RGBD_obj(char* fileName, SVzNL3DLaserLine* scanData, in sw.close(); } +void _outputScanDataFile_obj_vector(char* fileName, std::vector>scanData, + std::vector& objOps) +{ + int lineNum = scanData.size(); + std::ofstream sw(fileName); + int realLines = lineNum; + if (objOps.size() > 0) + 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 nTimeStamp = 0; + 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]; + int vType = pt3D->nPointIdx & 0xff; + int hType = vType >> 4; + int objId = (pt3D->nPointIdx >> 16) & 0xffff; + vType = vType & 0x0f; + if (LINE_FEATURE_L_JUMP_H2L == vType) + { + rgb = { 255, 97, 0 }; + size = 3; + } + else if (LINE_FEATURE_L_JUMP_L2H == vType) + { + rgb = { 255, 255, 0 }; + size = 3; + } + else if (LINE_FEATURE_V_SLOPE == vType) + { + rgb = { 255, 0, 255 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_H2L == vType) + { + rgb = { 160, 82, 45 }; + size = 3; + } + else if ((LINE_FEATURE_LINE_ENDING_0 == vType) || (LINE_FEATURE_LINE_ENDING_1 == vType)) + { + rgb = { 255, 0, 0 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_L2H == vType) + { + rgb = { 233, 150, 122 }; + size = 3; + } + else if (LINE_FEATURE_L_JUMP_H2L == hType) + { + rgb = { 0, 0, 255 }; + size = 3; + } + else if (LINE_FEATURE_L_JUMP_L2H == hType) + { + rgb = { 0, 255, 255 }; + size = 3; + } + else if (LINE_FEATURE_V_SLOPE == hType) + { + rgb = { 0, 255, 0 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_H2L == hType) + { + rgb = { 85, 107, 47 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_L2H == hType) + { + rgb = { 0, 255, 154 }; + size = 3; + } + else if ((LINE_FEATURE_LINE_ENDING_0 == hType) || (LINE_FEATURE_LINE_ENDING_1 == hType)) + { + rgb = { 255, 0, 0 }; + size = 3; + } + else if (objId > 0) //目标 + { + rgb = objColor[objId % 8]; + 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; + } + } + if (objOps.size() > 0) + { + int ptNum = objOps.size(); + sw << "Line_" << lineNum << "_0_" << ptNum << std::endl; + for (int i = 0; i < objOps.size(); i++) + { + if (i == 0) + rgb = { 255, 0, 0 }; + else + rgb = { 255, 255, 0 }; + size = 25; + float x = (float)objOps[i].centerPos.x; + float y = (float)objOps[i].centerPos.y; + float z = (float)objOps[i].centerPos.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; + } + } + } + //画出方向线 + sw.close(); +} + +void _outputRGBDScanDataFile_RGBD_obj(char* fileName, SVzNLXYZRGBDLaserLine* scanData, int lineNum, + float lineV, int maxTimeStamp, int clockPerSecond, std::vector& objOps) +{ + std::ofstream sw(fileName); + int realLines = lineNum; + if (objOps.size() > 0) + realLines++; + sw << "LineNum:" << realLines << std::endl; + sw << "DataType: 0" << std::endl; + sw << "ScanSpeed:" << lineV << std::endl; + sw << "PointAdjust: 1" << std::endl; + sw << "MaxTimeStamp:" << maxTimeStamp << "_" << clockPerSecond << 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 nTimeStamp = 0; + for (int line = 0; line < lineNum; line++) + { + sw << "Line_" << line << "_" << scanData[line].nTimeStamp << "_" << scanData[line].nPointCnt << std::endl; + nTimeStamp = scanData[line].nTimeStamp; + for (int i = 0; i < scanData[line].nPointCnt; i++) + { + SVzNLPointXYZRGBA* pt3D = &scanData[line].p3DPoint[i]; +#if 0 + int vType = pt3D->nRGB & 0xff; + int hType = vType >> 4; + int objId = (pt3D->nRGB >> 16) & 0xffff; + vType = vType & 0x0f; + if (LINE_FEATURE_L_JUMP_H2L == vType) + { + rgb = { 255, 97, 0 }; + size = 3; + } + else if (LINE_FEATURE_L_JUMP_L2H == vType) + { + rgb = { 255, 255, 0 }; + size = 3; + } + else if (LINE_FEATURE_V_SLOPE == vType) + { + rgb = { 255, 0, 255 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_H2L == vType) + { + rgb = { 160, 82, 45 }; + size = 3; + } + else if ((LINE_FEATURE_LINE_ENDING_0 == vType) || (LINE_FEATURE_LINE_ENDING_1 == vType)) + { + rgb = { 255, 0, 0 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_L2H == vType) + { + rgb = { 233, 150, 122 }; + size = 3; + } + else if (LINE_FEATURE_L_JUMP_H2L == hType) + { + rgb = { 0, 0, 255 }; + size = 3; + } + else if (LINE_FEATURE_L_JUMP_L2H == hType) + { + rgb = { 0, 255, 255 }; + size = 3; + } + else if (LINE_FEATURE_V_SLOPE == hType) + { + rgb = { 0, 255, 0 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_H2L == hType) + { + rgb = { 85, 107, 47 }; + size = 3; + } + else if (LINE_FEATURE_L_SLOPE_L2H == hType) + { + rgb = { 0, 255, 154 }; + size = 3; + } + else if ((LINE_FEATURE_LINE_ENDING_0 == hType) || (LINE_FEATURE_LINE_ENDING_1 == hType)) + { + rgb = { 255, 0, 0 }; + size = 3; + } + else if (objId > 0) //目标 + { + rgb = objColor[objId % 8]; + size = 5; + } + else +#endif + { + rgb = { 200, 200, 200 }; + size = 1; + } + float x = (float)pt3D->x; + float y = (float)pt3D->y; + float z = (float)pt3D->z; + sw << "{" << x << "," << y << "," << z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl; + } + } + if (objOps.size() > 0) + { + int ptNum = objOps.size(); + sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << ptNum << std::endl; + for (int i = 0; i < objOps.size(); i++) + { + if (i == 0) + rgb = { 255, 0, 0 }; + else + rgb = { 255, 255, 0 }; + size = 25; + float x = (float)objOps[i].centerPos.x; + float y = (float)objOps[i].centerPos.y; + float z = (float)objOps[i].centerPos.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; + } + } + } + sw.close(); +} + void _outputScanDataFile_RGBD_sideBagObj(char* fileName, SVzNL3DLaserLine* scanData, int lineNum, float lineV, int maxTimeStamp, int clockPerSecond, std::vector& objOps) { @@ -931,8 +1372,63 @@ void _rotateCloudPts(SVzNL3DLaserLine* scanData, int lineNum, double matrix3d[9] } } -void _XOYprojection(cv::Mat& img, std::vector>& dataLines, std::vector& objOps, - const double x_scale, const double y_scale, const SVzNLRangeD x_range, const SVzNLRangeD y_range, bool drawDirAngle) +void _rotateCloudPts_RGBD(SVzNLXYZRGBDLaserLine* scanData, int lineNum, double matrix3d[9], std::vector>& rotateLines, SVzNLRangeD* rx_range, SVzNLRangeD* ry_range) +{ + rx_range->min = 0; + rx_range->max = -1; + ry_range->min = 0; + ry_range->max = -1; + for (int line = 0; line < lineNum; line++) + { + std::vector< SVzNLPointXYZRGBA> linePts; + for (int i = 0; i < scanData[line].nPointCnt; i++) + { + SVzNLPointXYZRGBA* pt3D = &scanData[line].p3DPoint[i]; + if (pt3D->z < 1e-4) + continue; + + SVzNLPointXYZRGBA r_pt; + r_pt = _ptRotate_RGBD(*pt3D, matrix3d); + if (rx_range->max < rx_range->min) + { + rx_range->min = r_pt.x; + rx_range->max = r_pt.x; + } + else + { + if (rx_range->min > r_pt.x) + rx_range->min = r_pt.x; + if (rx_range->max < r_pt.x) + rx_range->max = r_pt.x; + } + if (ry_range->max < ry_range->min) + { + ry_range->min = r_pt.y; + ry_range->max = r_pt.y; + } + else + { + if (ry_range->min > r_pt.y) + ry_range->min = r_pt.y; + if (ry_range->max < r_pt.y) + ry_range->max = r_pt.y; + } + linePts.push_back(r_pt); + } + rotateLines.push_back(linePts); + } +} + +void _XOYprojection( + cv::Mat& img, + std::vector>& dataLines, + std::vector& objOps, + const double x_scale, + const double y_scale, + const SVzNLRangeD x_range, + const SVzNLRangeD y_range, + bool drawDirAngle, + int dirAngleLen) { int x_skip = 16; int y_skip = 16; @@ -1029,7 +1525,7 @@ void _XOYprojection(cv::Mat& img, std::vector>& da } else { - rgb = { 60, 60, 60 }; + rgb = { 150, 150, 150 }; size = 1; } @@ -1065,7 +1561,7 @@ void _XOYprojection(cv::Mat& img, std::vector>& da if (true == drawDirAngle) { //画线 - double R = 100; + double R = (double)dirAngleLen / 2.0; const double deg2rad = PI / 180.0; const double yaw = objOps[i].centerPos.z_yaw * deg2rad; double cy = cos(yaw); double sy = sin(yaw); @@ -1082,6 +1578,109 @@ void _XOYprojection(cv::Mat& img, std::vector>& da } +void _XOYprojection_RGBD( + cv::Mat& img, + std::vector>& dataLines, + std::vector& objOps, + const double x_scale, + const double y_scale, + const SVzNLRangeD x_range, + const SVzNLRangeD y_range, + bool drawDirAngle, + int dirAngleLen) +{ + int x_skip = 16; + int y_skip = 16; + + cv::Vec3b rgb = cv::Vec3b(0, 0, 0); + cv::Vec3b 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; + for (int line = 0; line < dataLines.size(); line++) + { + std::vector< SVzNLPointXYZRGBA>& a_line = dataLines[line]; + for (int i = 0; i < a_line.size(); i++) + { + SVzNLPointXYZRGBA* pt3D = &a_line[i]; + if (pt3D->z < 1e-4) + continue; + + int nRGB = pt3D->nRGB; + int r = nRGB & 0xff; + nRGB >>= 8; + int g = nRGB & 0xff; + nRGB >>= 8; + int b = nRGB & 0xff; + + rgb[0] = r; + rgb[1] = g; + rgb[2] = b; + size = 1; + double x = pt3D->x; + double y = pt3D->y; + int px = (int)((x - x_range.min) / x_scale + x_skip); + int py = (int)((y - y_range.min) / y_scale + y_skip); + if ((px == 666) && (py == 828)) + int kkk = 1; + if (size == 1) + img.at(py, px) = cv::Vec3b(rgb[2], rgb[1], rgb[0]); + else + cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); + } + } + if (objOps.size() > 0) + { + for (int i = 0; i < objOps.size(); i++) + { + if (i == 0) + { + rgb = { 255, 0, 0 }; + size = 20; + } + else + { + rgb = { 255, 255, 0 }; + size = 10; + } + int px = (int)((objOps[i].centerPos.x - x_range.min) / x_scale + x_skip); + int py = (int)((objOps[i].centerPos.y - y_range.min) / y_scale + y_skip); + cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); + if (true == drawDirAngle) + { + //画线 + double R = (double)dirAngleLen / 2.0; + const double deg2rad = PI / 180.0; + const double yaw = objOps[i].centerPos.z_yaw * deg2rad; + double cy = cos(yaw); double sy = sin(yaw); + double x1 = objOps[i].centerPos.x + R * cy; double y1 = objOps[i].centerPos.y - R * sy; + double x2 = objOps[i].centerPos.x - R * cy; double y2 = objOps[i].centerPos.y + R * sy; + int px1 = (int)((x1 - x_range.min) / x_scale + x_skip); + int py1 = (int)((y1 - y_range.min) / y_scale + y_skip); + int px2 = (int)((x2 - x_range.min) / x_scale + x_skip); + int py2 = (int)((y2 - y_range.min) / y_scale + y_skip); + cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 2); + if (objOps[i].orienFlag == 1) + { + cv::circle(img, cv::Point(px1, py1), 5, cv::Scalar(0, 255, 0), -1); + } + else if (objOps[i].orienFlag == 2) + { + cv::circle(img, cv::Point(px1, py1), 5, cv::Scalar(0, 0, 255), -1); + } + } + } + } + +} + void _XOYprojection_sideBagInfo(cv::Mat& img, std::vector>& dataLines, std::vector& objOps, const double x_scale, const double y_scale, const SVzNLRangeD x_range, const SVzNLRangeD y_range) { @@ -1250,7 +1849,7 @@ void _XOYprojection_sideBagInfo(cv::Mat& img, std::vector& objOps, double rpy[3]) +void _genXOYProjectionImage(cv::String& fileName, SVzNL3DLaserLine* scanData, int lineNum, std::vector& objOps, double rpy[3], double dirLen) { //统计X和Y的范围 std::vector> scan_lines; @@ -1302,7 +1901,13 @@ void _genXOYProjectionImage(cv::String& fileName, SVzNL3DLaserLine* scanData, in //计算投影比例 double x_scale = (x_range.max - x_range.min) / x_cols; double y_scale = (y_range.max - y_range.min) / y_rows; - _XOYprojection(img, scan_lines, objOps, x_scale, y_scale, x_range, y_range, true); + if (x_scale < y_scale) + x_scale = y_scale; + else + y_scale = x_scale; + + int angleDrawLen = dirLen / x_scale; + _XOYprojection(img, scan_lines, objOps, x_scale, y_scale, x_range, y_range, true, angleDrawLen); //旋转视角显示 double matrix3d[9]; @@ -1324,7 +1929,188 @@ void _genXOYProjectionImage(cv::String& fileName, SVzNL3DLaserLine* scanData, in //计算投影比例 double rx_scale = (rx_range.max - rx_range.min) / x_cols; double ry_scale = (ry_range.max - ry_range.min) / y_rows; - _XOYprojection(r_img, rotateLines, r_objOps, rx_scale, ry_scale, rx_range, ry_range, false); + _XOYprojection(r_img, rotateLines, r_objOps, rx_scale, ry_scale, rx_range, ry_range, false, angleDrawLen); + + cv::Mat dis_img; + cv::hconcat(img, r_img, dis_img); + cv::imwrite(fileName, dis_img); + return; +} + +void _RGBDto2DImage(char* fileName, SVzNLXYZRGBDLaserLine* scanData, int lineNum) +{ + cv::String imgName(fileName); + //统计X和Y的范围 + SVzNLRangeD x_range = { 0, -1 }; + SVzNLRangeD y_range = { 0, -1 }; + for (int line = 0; line < lineNum; line++) + { + for (int i = 0; i < scanData[line].nPointCnt; i++) + { + SVzNLPointXYZRGBA* pt3D = &scanData[line].p3DPoint[i]; + if (pt3D->z < 1e-4) + continue; + + if (x_range.max < x_range.min) + { + x_range.min = pt3D->x; + x_range.max = pt3D->x; + } + else + { + if (x_range.min > pt3D->x) + x_range.min = pt3D->x; + if (x_range.max < pt3D->x) + x_range.max = pt3D->x; + } + if (y_range.max < y_range.min) + { + y_range.min = pt3D->y; + y_range.max = pt3D->y; + } + else + { + if (y_range.min > pt3D->y) + y_range.min = pt3D->y; + if (y_range.max < pt3D->y) + y_range.max = pt3D->y; + } + } + } + + int x_skip = 16; + int y_skip = 16; + int imgRows = 992; + int imgCols = 1056; + double y_rows = (double)(imgRows - y_skip *2); + double x_cols = (double)(imgCols - x_skip *2); + cv::Mat img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); + //计算投影比例 + double x_scale = (x_range.max - x_range.min) / x_cols; + double y_scale = (y_range.max - y_range.min) / y_rows; + if (x_scale < y_scale) + x_scale = y_scale; + else + y_scale = x_scale; + + int size = 1; + cv::Vec3b rgb = cv::Vec3b(0, 0, 0); + for (int line = 0; line < lineNum; line++) + { + for (int i = 0; i < scanData[line].nPointCnt; i++) + { + SVzNLPointXYZRGBA* pt3D = &scanData[line].p3DPoint[i]; + if (pt3D->z < 1e-4) + continue; + + int nRGB = pt3D->nRGB; + int r = nRGB & 0xff; + nRGB >>= 8; + int g = nRGB & 0xff; + nRGB >>= 8; + int b = nRGB & 0xff; + + rgb[0] = r; + rgb[1] = g; + rgb[2] = b; + size = 1; + double x = pt3D->x; + double y = pt3D->y; + int px = (int)((x - x_range.min) / x_scale + x_skip); + int py = (int)((y - y_range.min) / y_scale + y_skip); + if ((px == 666) && (py == 828)) + int kkk = 1; + if (size == 1) + img.at(py, px) = cv::Vec3b(rgb[2], rgb[1], rgb[0]); + else + cv::circle(img, cv::Point(px, py), size, cv::Scalar(rgb[2], rgb[1], rgb[0]), -1); + } + } + cv::imwrite(imgName, img); + return; +} + +void _genXOYProjectionImage_RGBD(cv::String& fileName, SVzNLXYZRGBDLaserLine* scanData, int lineNum, std::vector& objOps, double rpy[3], double dirLen) +{ + //统计X和Y的范围 + std::vector> scan_lines; + SVzNLRangeD x_range = { 0, -1 }; + SVzNLRangeD y_range = { 0, -1 }; + for (int line = 0; line < lineNum; line++) + { + std::vector< SVzNLPointXYZRGBA> a_line; + for (int i = 0; i < scanData[line].nPointCnt; i++) + { + SVzNLPointXYZRGBA* pt3D = &scanData[line].p3DPoint[i]; + if (pt3D->z < 1e-4) + continue; + + a_line.push_back(*pt3D); + if (x_range.max < x_range.min) + { + x_range.min = pt3D->x; + x_range.max = pt3D->x; + } + else + { + if (x_range.min > pt3D->x) + x_range.min = pt3D->x; + if (x_range.max < pt3D->x) + x_range.max = pt3D->x; + } + if (y_range.max < y_range.min) + { + y_range.min = pt3D->y; + y_range.max = pt3D->y; + } + else + { + if (y_range.min > pt3D->y) + y_range.min = pt3D->y; + if (y_range.max < pt3D->y) + y_range.max = pt3D->y; + } + } + scan_lines.push_back(a_line); + } + + int imgRows = 992; + int imgCols = 1056; + double y_rows = 960.0; + double x_cols = 1024.0; + cv::Mat img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); + //计算投影比例 + double x_scale = (x_range.max - x_range.min) / x_cols; + double y_scale = (y_range.max - y_range.min) / y_rows; + if (x_scale < y_scale) + x_scale = y_scale; + else + y_scale = x_scale; + + int angleDrawLen = dirLen / x_scale; + _XOYprojection_RGBD(img, scan_lines, objOps, x_scale, y_scale, x_range, y_range, true, angleDrawLen); + + //旋转视角显示 + double matrix3d[9]; + EulerRpyToRotation1(rpy, matrix3d); + std::vector r_objOps; + r_objOps.insert(r_objOps.end(), objOps.begin(), objOps.end()); + for (int i = 0; i < objOps.size(); i++) + { + SVzNL3DPoint c_pt = { objOps[i].centerPos.x, objOps[i].centerPos.y, objOps[i].centerPos.z }; + SVzNL3DPoint r_c_pt = _ptRotate(c_pt, matrix3d); + r_objOps[i].centerPos.x = r_c_pt.x; + r_objOps[i].centerPos.y = r_c_pt.y; + r_objOps[i].centerPos.z = r_c_pt.z; + } + std::vector> rotateLines; + SVzNLRangeD rx_range, ry_range; + _rotateCloudPts_RGBD(scanData, lineNum, matrix3d, rotateLines, &rx_range, &ry_range); + cv::Mat r_img = cv::Mat::zeros(imgRows, imgCols, CV_8UC3); + //计算投影比例 + double rx_scale = (rx_range.max - rx_range.min) / x_cols; + double ry_scale = (ry_range.max - ry_range.min) / y_rows; + _XOYprojection_RGBD(r_img, rotateLines, r_objOps, rx_scale, ry_scale, rx_range, ry_range, false, angleDrawLen); cv::Mat dis_img; cv::hconcat(img, r_img, dis_img); @@ -1614,11 +2400,36 @@ void _outputScanDataFile_removeZeros(char* fileName, SVzNL3DLaserLine* scanData, } #define TEST_CONVERT_TO_GRID 0 +#define TEST_CONVERT_RGBD_TO_GRID 0 #define TEST_COMPUTE_GRASP_POINT 1 -#define TEST_COMPUTE_CALIB_PARA 1 -#define TEST_GROUP 16 +#define TEST_COMPUTE_CALIB_PARA 0 + +#define TEST_GROUP 21 +#define TEST_TOP_VIEW_GROUP (TEST_GROUP - 8) +#define TEST_TOP_ORIEN_GROUP (TEST_GROUP - 6) int main() { +#if TEST_CONVERT_RGBD_TO_GRID + char _scan_dir[256]; + sprintf_s(_scan_dir, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\"); + char _in_file[256]; + double _F = 1247.95;// 1231.2; //1729.0;; //f + for (int i = 1; i <= 28; i++) + { + sprintf_s(_in_file, "%sLaserLine%d.txt", _scan_dir, i); + std::vector> scanData; + vzReadLaserScanPointFromFile_XYZRGB_vector(_in_file, scanData); + if (scanData.size() == 0) + continue; + + std::vector> gridData; + _convertToGridData_XYZRGB_vector(scanData, _F, gridData); + char _out_file[256]; + sprintf_s(_out_file, "%s%d-RGB点云.txt", _scan_dir, i); + _outputScanDataFile_SPointXYZRGB_vector(_out_file, gridData); + printf("%s: convert done!\n", _in_file); + } +#endif #if TEST_CONVERT_TO_GRID //将数据转换成栅格格式格式 char _scan_dir[256]; @@ -1627,17 +2438,17 @@ int main() int dataCalib = 0; int maxTimeStamp = 0; int clockPerSecond = 0; - sprintf_s(_scan_dir, "F:\\上古\\编织袋数据\\侧抓数据\\"); + sprintf_s(_scan_dir, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\"); char _scan_file[256]; - double _F = 1248.5;// 1231.2; //1729.0;; //f - for (int i = 1; i <= 2; i++) + double _F = 1247.95;// 1231.2; //1729.0;; //f + for (int i = 1; i <= 28; i++) { - sprintf_s(_scan_file, "%s%d_LaserData_Hi228621.txt", _scan_dir, i); + sprintf_s(_scan_file, "%sLaserLine%d.txt", _scan_dir, i); SVzNLXYZRGBDLaserLine* laser3DPoints_RGBD = vzReadLaserScanPointFromFile_XYZRGB(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond); if (laser3DPoints_RGBD == NULL) continue; -#if 1 +#if 0 double rpy[3] = { 0,-18, 0 }; double camPoseR[9]; EulerRpyToRotation1(rpy, camPoseR); @@ -1682,7 +2493,7 @@ int main() #if TEST_COMPUTE_CALIB_PARA char _calib_datafile[256]; - sprintf_s(_calib_datafile, "F:\\上古\\编织袋数据\\点云8_广东1213-1215数据\\LaserLine10_grid.txt"); + sprintf_s(_calib_datafile, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\底面点云\\底面点云.txt"); int lineNum = 0; float lineV = 0.0f; int dataCalib = 0; @@ -1701,14 +2512,14 @@ int main() int kkk = 1; //行处理 //调平，去除地面 - sg_lineDataR(&laser3DPoints[i], calibPara.planeCalib, calibPara.planeHeight); + sg_lineDataR(&laser3DPoints[i], calibPara.planeCalib, -1);// calibPara.planeHeight); } // char calibFile[250]; - sprintf_s(calibFile, "F:\\上古\\编织袋数据\\点云8_广东1213-1215数据\\ground_calib_para.txt"); + sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\ground_calib_para.txt"); _outputCalibPara(calibFile, calibPara); char _out_file[256]; - sprintf_s(_out_file, "F:\\上古\\编织袋数据\\点云8_广东1213-1215数据\\LaserLine10_grid_calib.txt"); + sprintf_s(_out_file, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\LaserLine1_grid_calib.txt"); _outputScanDataFile_self(_out_file, laser3DPoints, lineNum, lineV, maxTimeStamp, clockPerSecond); printf("%s: calib done!\n", _calib_datafile); @@ -1728,17 +2539,23 @@ int main() "F:\\ShangGu\\编织袋数据\\点云8_广东1213-1215数据\\", //7 "F:\\ShangGu\\编织袋数据\\点云9_广东1219数据\\", //8 "F:\\ShangGu\\编织袋数据\\点云10\\", //9 - "F:\\ShangGu\\编织袋数据\\侧抓数据\\", //10 - "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250419\\", //11 - "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-1\\", //12 - "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-2\\", //13 - "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-3\\", //14 - "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-4\\", //15 + "F:\\ShangGu\\编织袋数据\\点云11_盐袋\\", //10 + "F:\\ShangGu\\编织袋数据\\点云12_盐袋\\", //11 + "F:\\ShangGu\\编织袋数据\\点云13_现场测试\\", //12 + "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\", //13 + "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\", //14 + "F:\\ShangGu\\编织袋数据\\侧抓数据\\", //15 + "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250419\\", //16 + "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-1\\", //17 + "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-2\\", //18 + "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-3\\", //19 + "F:\\ShangGu\\编织袋数据\\侧抓数据_现场\\20250420-4\\", //20 }; SVzNLRange fileIdx[TEST_GROUP] = { {0,176},{1,200},{1,166},{122,141},{1,65}, {1,29},{108,135},{0,200}, {1,200}, {1,12}, + {2,4}, {1,5}, {1,1}, {1,21},{1,28}, {3,3}, {1,51}, {4,83}, {1,74}, {1,61}, {1,84} }; @@ -1758,58 +2575,126 @@ int main() for (int i = 0; i < 9; i++) poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i]; - SG_bagPositionParam algoParam; - algoParam.bagParam.bagL = 650; //袋子长65cm - algoParam.bagParam.bagW = 450; //袋子宽40cm - algoParam.bagParam.bagH = 160; //袋子高16cm - -#if BAG_ALGO_USE_CORNER_FEATURE - algoParam.cornerParam.cornerTh = 30; //45度角 - algoParam.cornerParam.scale = 15; // 30; // algoParam.bagParam.bagH / 8; - algoParam.cornerParam.minEndingGap = algoParam.bagParam.bagW / 4; - algoParam.cornerParam.jumpCornerTh_1 = 60; - algoParam.cornerParam.jumpCornerTh_2 = 15; -#else - algoParam.slopeParam.LSlopeZWin = 10.0; - algoParam.slopeParam.validSlopeH = 10.0; - algoParam.slopeParam.minLJumpH = 20.0; - algoParam.slopeParam.minEndingGap = algoParam.bagParam.bagW / 4; ///当没有中间袋子，只有左右袋子时，有两组Ending。此参数确定两个ending最小间距 - algoParam.valleyPara.valleyMinH = 10.0; - algoParam.valleyPara.valleyMaxW = 80.0; //取袋子宽度的1/5 -#endif - algoParam.growParam.maxLineSkipNum = 5; - algoParam.growParam.yDeviation_max = 20.0; - algoParam.growParam.maxSkipDistance = 20.0; - algoParam.growParam.zDeviation_max = 80.0; //袋子高度1/2 - algoParam.growParam.minLTypeTreeLen = 50.0; //mm - algoParam.growParam.minVTypeTreeLen = 50.0; //mm - char _scan_file[256]; + SG_bagPositionParam algoParam; int endGroup = TEST_GROUP - 1; - for (int grp = 0; grp <= 9; grp++) + for (int grp = 14; grp <= 14; grp++) { + if (grp < 10) + { + algoParam.bagParam.bagL = 650; //袋子长65cm + algoParam.bagParam.bagW = 450; //袋子宽40cm + algoParam.bagParam.bagH = 160; //袋子高16cm + algoParam.growParam.maxLineSkipNum = 5; + algoParam.growParam.yDeviation_max = 20.0; + algoParam.growParam.maxSkipDistance = 20.0; + algoParam.growParam.zDeviation_max = algoParam.bagParam.bagH / 2; //袋子高度1/2 + algoParam.growParam.minLTypeTreeLen = 50.0; //mm + algoParam.growParam.minVTypeTreeLen = 50.0; //mm + } + else if ( (grp >= 10) && (grp <= 11)) + { + algoParam.bagParam.bagL = 150; //袋子长65cm + algoParam.bagParam.bagW = 110; //袋子宽40cm + algoParam.bagParam.bagH = 20; //袋子高16cm + algoParam.growParam.maxLineSkipNum = 5; + algoParam.growParam.yDeviation_max = 20.0; + algoParam.growParam.maxSkipDistance = 20.0; + algoParam.growParam.zDeviation_max = algoParam.bagParam.bagH / 2; //袋子高度1/2 + algoParam.growParam.minLTypeTreeLen = 50.0; //mm + algoParam.growParam.minVTypeTreeLen = 50.0; //mm + } + else if ( (grp >= 13) && (grp <= 14)) + { + algoParam.bagParam.bagL = 750; //袋子长65cm + algoParam.bagParam.bagW = 450; //袋子宽40cm + algoParam.bagParam.bagH = 160; //袋子高16cm + algoParam.growParam.maxLineSkipNum = 5; + algoParam.growParam.yDeviation_max = 20.0; + algoParam.growParam.maxSkipDistance = 20.0; + algoParam.growParam.zDeviation_max = algoParam.bagParam.bagH / 2; //袋子高度1/2 + algoParam.growParam.minLTypeTreeLen = 50.0; //mm + algoParam.growParam.minVTypeTreeLen = 50.0; //mm + } + else + { + algoParam.bagParam.bagL = 650; //袋子长65cm + algoParam.bagParam.bagW = 450; //袋子宽40cm + algoParam.bagParam.bagH = 160; //袋子高16cm + algoParam.growParam.maxLineSkipNum = 5; + algoParam.growParam.yDeviation_max = 20.0; + algoParam.growParam.maxSkipDistance = 20.0; + algoParam.growParam.zDeviation_max = algoParam.bagParam.bagH / 2; //袋子高度1/2 + algoParam.growParam.minLTypeTreeLen = 50.0; //mm + algoParam.growParam.minVTypeTreeLen = 50.0; //mm + } +#if BAG_ALGO_USE_CORNER_FEATURE + algoParam.cornerParam.cornerTh = 30; //45度角 + algoParam.cornerParam.scale = algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8; + algoParam.cornerParam.minEndingGap = algoParam.bagParam.bagW / 4; + algoParam.cornerParam.jumpCornerTh_1 = 60; + algoParam.cornerParam.jumpCornerTh_2 = 15; +#else + algoParam.slopeParam.LSlopeZWin = 10.0; + algoParam.slopeParam.validSlopeH = 10.0; + algoParam.slopeParam.minLJumpH = 20.0; + algoParam.slopeParam.minEndingGap = algoParam.bagParam.bagW / 4; ///当没有中间袋子，只有左右袋子时，有两组Ending。此参数确定两个ending最小间距 + algoParam.valleyPara.valleyMinH = 10.0; + algoParam.valleyPara.valleyMaxW = 80.0; //取袋子宽度的1/5 +#endif + if (grp == 7) { char calibFile[250]; sprintf_s(calibFile, "F:\\上古\\编织袋数据\\点云8_广东1213-1215数据\\ground_calib_para.txt"); poseCalibPara = _readCalibPara(calibFile); } + else if (grp == 10) + { + char calibFile[250]; + sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\点云11_盐袋\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + } + else if (grp == 11) + { + char calibFile[250]; + sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\点云12_盐袋\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + } + else if (grp == 12) + { + char calibFile[250]; + sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\点云13_现场测试\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + } + else if ( grp == 13) + { + char calibFile[250]; + sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + } + else if (grp == 14) + { + char calibFile[250]; + sprintf_s(calibFile, "F:\\ShangGu\\编织袋数据\\编织袋RGBD识别方向_2\\ground_calib_para.txt"); + poseCalibPara = _readCalibPara(calibFile); + } for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { - //fidx = 102; - int lineNum = 0; - float lineV = 0.0f; - int dataCalib = 0; - int maxTimeStamp = 0; - int clockPerSecond = 0; - 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) - continue; - - if (grp < 10) //正面抓取 + fidx = 10; + if (grp < TEST_TOP_VIEW_GROUP) //正面抓取 { + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + 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) + continue; + algoParam.filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声 algoParam.filterParam.outlierTh = 5; @@ -1851,7 +2736,8 @@ int main() 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, objOps, rpy); + double angleDrawLen = algoParam.bagParam.bagL / 3; + _genXOYProjectionImage(imgName, laser3DPoints, lineNum, objOps, rpy, angleDrawLen); #endif #if 0 //量化成2D图像作为训练样本 @@ -1872,8 +2758,131 @@ int main() #endif printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); } - else if (grp >= 10) //侧面抓取 + else if (grp < TEST_TOP_ORIEN_GROUP) { + double FBVldPtRatioTh; + + SSG_hsvCmpParam colorCmpParam; //色度和色饱和度比较门限，小于门限为同一颜色 + colorCmpParam.hueTh = 15.0; + colorCmpParam.saturateTh = 120.0; //60 + colorCmpParam.FBVldPtRatioTh = 0.075; //正反两面有效颜色点的比例门限 + colorCmpParam.frontVldPtGreater = true; //true：有效颜色比例高的点的是正面；false：有效颜色比例高的点的是反面 + colorCmpParam.front_upVldPtGreater = false;//true：有效颜色比例高的点的是正面朝上；false：有效颜色比例高的点的是正面朝下 + colorCmpParam.back_upVldPtGreater = true; //true：有效颜色比例高的点的是反面朝上；false：有效颜色比例高的点的是反面朝下 + RGB rgbColorPattern = {36, 165, 208}; //用于区分袋子方向的颜色 + double frontColorTemplate[RGN_HIST_SIZE] = { + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 + }; + + double backColorTemplate[RGN_HIST_SIZE] = { + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 + }; + + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + sprintf_s(_scan_file, "%s%d-RGB点云.txt", dataPath[grp], fidx); + SVzNLXYZRGBDLaserLine* laser3DPoints = vzReadLaserScanPointFromFile_XYZRGB( + _scan_file, + &lineNum, + &lineV, + &dataCalib, + &maxTimeStamp, + &clockPerSecond); + if (laser3DPoints == NULL) + continue; + + algoParam.filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声 + algoParam.filterParam.outlierTh = 5; + + long t1 = GetTickCount64(); + + for (int i = 0; i < lineNum; i++) + { + if (i == 14) + int kkk = 1; + //行处理 + //调平，去除地面 + sg_lineDataR_RGBD(&laser3DPoints[i], poseCalibPara.planeCalib, poseCalibPara.planeHeight); + } +#if 0 + //产生一个调平后的2D图像，核对颜色 + char _tst_file[256]; + sprintf_s(_tst_file, "%sresult\\LaserLine%d_2D_img.png", dataPath[grp], fidx); + _RGBDto2DImage(_tst_file, laser3DPoints, lineNum); +#endif + int errCode = 0; + std::vector objOps; +#if OUTPUT_DEBUG + std::vector> debug_bagPositionCloud; +#endif + sg_getBagPositionAndOrientation( + laser3DPoints, + lineNum, + algoParam, + poseCalibPara, + colorCmpParam, + rgbColorPattern, + frontColorTemplate, + backColorTemplate, + objOps, +#if OUTPUT_DEBUG + debug_bagPositionCloud, +#endif + &errCode); + + long t2 = GetTickCount64(); + + char _dbg_file[256]; +#if 1 +#if OUTPUT_DEBUG + sprintf_s(_dbg_file, "%sresult\\LaserLine%d_bagPosition_result.txt", dataPath[grp], fidx); + _outputScanDataFile_obj_vector(_dbg_file, debug_bagPositionCloud, objOps); + +#endif + + sprintf_s(_dbg_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx); + _outputRGBDScanDataFile_RGBD_obj(_dbg_file, laser3DPoints, lineNum, lineV, maxTimeStamp, clockPerSecond, objOps); + + 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 }; // + double angleDrawLen = algoParam.bagParam.bagL / 3; + _genXOYProjectionImage_RGBD(imgName, laser3DPoints, lineNum, objOps, rpy, angleDrawLen); +#endif +#if 0 + //量化成2D图像作为训练样本 + cv::Mat project2D_img; + double z_scale = 3.0; + double xy_scale = 2.0; + project2DWithInterpolate(project2D_img, laser3DPoints, lineNum, xy_scale, z_scale); + cv::Mat project2D_color; + cv::applyColorMap(project2D_img, project2D_color, cv::COLORMAP_JET); + sprintf_s(_dbg_file, "%simg2D_JET\\LaserLine%d_img2D.png", dataPath[grp], fidx); + cv::String img2DName(_dbg_file); + cv::imwrite(img2DName, project2D_color); + cv::Mat project2D_color_HOT; + cv::applyColorMap(project2D_img, project2D_color_HOT, cv::COLORMAP_HOT); + sprintf_s(_dbg_file, "%simg2D\\LaserLine%d_img2D.png", dataPath[grp], fidx); + cv::String img2DName_HOT(_dbg_file); + cv::imwrite(img2DName_HOT, project2D_color_HOT); +#endif + printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1)); + } + else// if (grp >= TEST_TOP_VIEW_GROUP) //侧面抓取 + { + int lineNum = 0; + float lineV = 0.0f; + int dataCalib = 0; + int maxTimeStamp = 0; + int clockPerSecond = 0; + 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) + continue; + algoParam.filterParam.continuityTh = 5.0; //噪声滤除。当相邻点的距离大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声 algoParam.filterParam.outlierTh = 5; diff --git a/baseAlgorithm/baseAlgorithm.vcxproj b/baseAlgorithm/baseAlgorithm.vcxproj index 42437db..de7d070 100644 --- a/baseAlgorithm/baseAlgorithm.vcxproj +++ b/baseAlgorithm/baseAlgorithm.vcxproj @@ -166,6 +166,7 @@ + diff --git a/sourceCode/SG_bagPositioning.cpp b/sourceCode/SG_bagPositioning.cpp index fbb8e6f..c52dd4b 100644 --- a/sourceCode/SG_bagPositioning.cpp +++ b/sourceCode/SG_bagPositioning.cpp @@ -12,6 +12,13 @@ void sg_lineDataR(SVzNL3DLaserLine* a_line, lineDataRT( a_line, camPoseR, groundH); } +void sg_lineDataR_RGBD(SVzNLXYZRGBDLaserLine* a_line, + const double* camPoseR, + double groundH) +{ + lineDataRT_RGBD(a_line, camPoseR, groundH); +} + //ɨߴдֱȡ void sg_bagPositioning_lineProc( SVzNL3DLaserLine* a_line, @@ -2289,7 +2296,7 @@ void sg_getBagPosition( cv::imwrite("originMask.png", oriMaskImage); #endif - int vldRgnSize = (int)((algoParam.bagParam.bagL / 2) * (algoParam.bagParam.bagW / 2) / (scale * scale)); + int vldRgnSize = (int)((algoParam.bagParam.bagL * algoParam.bagParam.bagW / 2) / (scale * scale)); for (int rgnid = 0, rgn_max = labelRgns.size(); rgnid < rgn_max; rgnid++) { SSG_Region* a_rgn = &labelRgns[rgnid]; @@ -3807,6 +3814,440 @@ if (i == 2) } } +typedef struct +{ + int vldNum[RGN_HIST_SIZE]; + int totalNum[RGN_HIST_SIZE]; +}_rgnHSVHistInfo; + +HSV RGBtoHSV(int r, int g, int b) { + double r_ = r / 255.0; + double g_ = g / 255.0; + double b_ = b / 255.0; + double max_ = std::max(r_, std::max(g_, b_)); + double min_ = std::min(r_, std::min(g_, b_)); + double delta = max_ - min_; + double h, s, v; + v = max_; + s = (max_ == 0) ? 0 : (delta / max_); + if (delta == 0) //˴޸ġ趨һ + { + h = -1; // δ壬κֵͨΪ0NaN + } + else + { + if (r_ == max_) { + h = (g_ - b_) / delta; + } + else if (g_ == max_) { + h = 2 + (b_ - r_) / delta; + } + else { // b_ == max_ + h = 4 + (r_ - g_) / delta; + } + h *= 60; // hֵ׼[0,360] + if (h < 0) { + h += 360; // ȷh[0,360]Χ + } + } + HSV hsv; + hsv.h = h; + hsv.s = s * 255;// sֵ׼[0,255] + hsv.v = v * 255;// vֵ׼[0,255] + return hsv; +} + +bool isSameColor(HSV hsvPattern, HSV hsvPt, const SSG_hsvCmpParam colorCmpParam) +{ + if (((hsvPattern.h < 0) && (hsvPt.h >= 0)) || ((hsvPattern.h >= 0) && (hsvPt.h < 0))) + return false; + + if ((hsvPattern.h < 0) && (hsvPt.h < 0)) + return true; + + double h_diff = hsvPt.h - hsvPt.h; + if (h_diff < -180) + h_diff = h_diff + 360; + else if (h_diff > 180) + h_diff = h_diff - 360; + h_diff = abs(h_diff); + double s_diff = abs(hsvPattern.s - hsvPt.s); + if ( (h_diff < colorCmpParam.hueTh) && (s_diff < colorCmpParam.saturateTh)) + return true; + else + return false; +} + +double cosSimilarity(const double* colorTemplate, double* sampleData) +{ + double sumA = 0, sumB = 0, sumAB = 0; + for (int i = 0; i < RGN_HIST_SIZE; i++) + { + sumA += colorTemplate[i] * colorTemplate[i]; + sumAB += colorTemplate[i] * sampleData[i]; + sumB = sampleData[i] * sampleData[i]; + } + sumA = sqrt(sumA); + sumB = sqrt(sumB); + sumA = sumA * sumB; + if (sumA <= 1e-4) + return 0; + return (sumAB / sumA); +} + +double cosSimilarityInv(const double* colorTemplate, double* sampleData) +{ + double sumA = 0, sumB = 0, sumAB = 0; + for (int i = 0; i < RGN_HIST_SIZE; i++) + { + sumA += colorTemplate[i] * colorTemplate[i]; + sumAB += colorTemplate[i] * sampleData[RGN_HIST_SIZE - 1 - i]; + sumB = sampleData[RGN_HIST_SIZE - 1 - i] * sampleData[RGN_HIST_SIZE -1 - i]; + } + sumA = sqrt(sumA); + sumB = sqrt(sumB); + sumA = sumA * sumB; + if (sumA <= 1e-4) + return 0; + return (sumAB / sumA); +} + +///RGBD gridʽԽˮƽʹֱĴ +///1Ѱұ߽ +///2ߵ㿪ʼ +/// ֯³ +void sg_getBagPositionAndOrientation( + SVzNLXYZRGBDLaserLine* laser3DPoints, + int lineNum, + //std::vector& all_vLineFeatures, + //std::vector>& noisePts, + const SG_bagPositionParam algoParam, + const SSG_planeCalibPara poseCalibPara, + const SSG_hsvCmpParam colorCmpParam, + const RGB rgbColorPattern, //ִӷɫ + const double frontColorTemplate[RGN_HIST_SIZE], + const double backColorTemplate[RGN_HIST_SIZE], + std::vector& objOps, +#if OUTPUT_DEBUG + std::vector>& bagPositionCloudPts, +#endif + int* errCode) +{ + *errCode = 0; + if (lineNum == 0) + { + *errCode = SG_ERR_3D_DATA_INVLD; + return; + } + //ǷΪդʽ + //RGBDƷӦRGB͵ƣRGBתHSV + std::vector cloudPts; + std::vector> hsvPts; + bool dataValid = true; + int linePtNum = laser3DPoints[0].nPointCnt; + for (int line = 0; line < lineNum; line++) + { + if (laser3DPoints[line].nPointCnt > 0) + { + std::vector line_hsv; + line_hsv.resize(laser3DPoints[line].nPointCnt); + SVzNL3DPosition* p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * laser3DPoints[line].nPointCnt); + memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * laser3DPoints[line].nPointCnt); + for (int i = 0; i < laser3DPoints[line].nPointCnt; i++) + { + if ((line == 497) && (i >= 902)) + int kkk = 1; + p3DPoint[i].pt3D.x = laser3DPoints[line].p3DPoint[i].x; + p3DPoint[i].pt3D.y = laser3DPoints[line].p3DPoint[i].y; + p3DPoint[i].pt3D.z = laser3DPoints[line].p3DPoint[i].z; + p3DPoint[i].nPointIdx = i; + + unsigned int nRGB = laser3DPoints[line].p3DPoint[i].nRGB; + + int r = nRGB & 0xff; + nRGB >>= 8; + int g = nRGB & 0xff; + nRGB >>= 8; + int b = nRGB & 0xff; + line_hsv[i] = RGBtoHSV(r,g,b); + } + + SVzNL3DLaserLine a_line; + a_line.nPositionCnt = laser3DPoints[line].nPointCnt; + a_line.nTimeStamp = 0; + a_line.p3DPosition = p3DPoint; + cloudPts.push_back(a_line); + hsvPts.push_back(line_hsv); + } + if (linePtNum != laser3DPoints[line].nPointCnt) + dataValid = false; + } + + if (false == dataValid) + { + *errCode = SG_ERR_3D_DATA_INVLD; + //ͷڴ + for (int i = 0, i_max = cloudPts.size(); i < i_max; i++) + free(cloudPts[i].p3DPosition); + return; + } + + //Ŀ + std::vector peakRgn; + sg_getBagPosition( + &cloudPts[0], + cloudPts.size(), + algoParam, + poseCalibPara, + peakRgn); + + //ͷڴ + for (int i = 0, i_max = cloudPts.size(); i < i_max; i++) + { +#if OUTPUT_DEBUG + std::vector< SVzNL3DPosition> out_line; + int nPtCounter = cloudPts[i].nPositionCnt; + for (int j = 0; j < nPtCounter; j++) + out_line.push_back(cloudPts[i].p3DPosition[j]); + bagPositionCloudPts.push_back(out_line); +#endif + free(cloudPts[i].p3DPosition); + cloudPts[i].p3DPosition = NULL; + } + + //ͶԭϵԱ۱궨ȷ + for (int i = 0; i < lineNum; i++) + sg_lineDataR_RGBD(&laser3DPoints[i], poseCalibPara.invRMatrix, -1); + + //ɫתHSV + HSV hsvPattern = RGBtoHSV(rgbColorPattern.r, rgbColorPattern.g, rgbColorPattern.b); + //ɫͳƴȡcenterPosΪĵģ0.8*Width, 0.8*HeightķΧڵĵͳ + int objNum = peakRgn.size(); + if (objNum > 0) + { + //ˮƽõľΪ׼ģλãˮƽΪȣֱΪ + std::vector y_th; + y_th.resize(objNum); + std::vector x_th; + x_th.resize(objNum); + std::vector x_slice; + x_slice.resize(objNum); + for (int m = 0; m < objNum; m++) + { + y_th[m] = peakRgn[m].objSize.dHeight * 0.25; + x_th[m] = peakRgn[m].objSize.dWidth * 0.45; + x_slice[m] = peakRgn[m].objSize.dWidth / 16.0; + } + + std::vector rgnROIs; + rgnROIs.resize(objNum); + for (int i = 0; i < objNum; i++) + { + //һROIٴ + //ˮƽõľΪ׼ģλ + SVzNL2DPointD vec[4]; + vec[0].x = -peakRgn[i].objSize.dWidth / 2; + vec[0].y = -peakRgn[i].objSize.dHeight / 2; + vec[1].x = peakRgn[i].objSize.dWidth / 2; + vec[1].y = -peakRgn[i].objSize.dHeight / 2; + vec[2].x = peakRgn[i].objSize.dWidth / 2; + vec[2].y = peakRgn[i].objSize.dHeight / 2; + vec[3].x = -peakRgn[i].objSize.dWidth / 2; + vec[3].y = peakRgn[i].objSize.dHeight / 2; + //ת + const double deg2rad = PI / 180.0; + const double yaw = peakRgn[i].centerPos.z_yaw * deg2rad; + double cy = cos(yaw); double sy = sin(yaw); + for (int m = 0; m < 4; m++) + { + double x = vec[m].x * cy + vec[m].y * sy; //ͼϵYŷϵ෴ + double y = -vec[m].x * sy + vec[m].y * cy; + vec[m].x = x + peakRgn[i].centerPos.x; + vec[m].y = y + peakRgn[i].centerPos.y; + } + //ROI + SSG_ROIRectD a_roi; + a_roi.left = vec[0].x; + a_roi.right = vec[0].x; + a_roi.top = vec[0].y; + a_roi.bottom = vec[0].y; + for (int m = 1; m < 4; m++) + { + a_roi.left = a_roi.left > vec[m].x ? vec[m].x : a_roi.left; + a_roi.right = a_roi.right < vec[m].x ? vec[m].x : a_roi.right; + a_roi.top = a_roi.top > vec[m].y ? vec[m].y : a_roi.top; + a_roi.bottom = a_roi.bottom < vec[m].y ? vec[m].y : a_roi.bottom; + } + rgnROIs[i] = a_roi; + } + + std::vector<_rgnHSVHistInfo> rgnHists; //ͳ + rgnHists.resize(objNum); + //ͳ + for (int m = 0; m < objNum; m++) + { + for (int j = 0; j < 16; j++) + { + rgnHists[m].totalNum[j] = 0; + rgnHists[m].vldNum[j] = 0; + } + } + + //еͶ䣬ע˴ʹԭʼ + for (int line = 0; line < lineNum; line++) + { + for (int j = 0; j < linePtNum; j++) + { + SVzNLPointXYZRGBA* a_pt = &laser3DPoints[line].p3DPoint[j]; + if (a_pt->z < 1e-4) + continue; + if ((line == 497) && (j >= 902)) + { + HSV a_hsv = hsvPts[line][j]; + int kkk = 1; + } + for (int m = 0; m < objNum; m++) + { + //ǷROI + if ((a_pt->x >= rgnROIs[m].left) && (a_pt->x <= rgnROIs[m].right) && + (a_pt->y >= rgnROIs[m].top) && (a_pt->y <= rgnROIs[m].bottom)) + { + //Ͷ + //ת,ʱǶ + const double deg2rad = PI / 180.0; + double rotateAngle = - peakRgn[m].centerPos.z_yaw * deg2rad; + double cy = cos(rotateAngle); double sy = sin(rotateAngle); + double x = a_pt->x - peakRgn[m].centerPos.x; + double y = a_pt->y - peakRgn[m].centerPos.y; + double rx = x * cy + y * sy; + double ry = -x * sy + y * cy; + if ((rx >= -x_th[m]) && (rx <= x_th[m]) && (ry >= -y_th[m]) && (ry <= y_th[m])) + { + if (m == 0) + int kkk = 1; + else if (m == 1) + int kkk = 1; + else if (m == 2) + int kkk = 1; + else if (m == 3) + int kkk = 1; + double rx_abs = abs(rx); + int hist_idx = rx_abs / x_slice[m]; + if (hist_idx >= RGN_HIST_SIZE / 2) + hist_idx = RGN_HIST_SIZE / 2 - 1; + if (rx < 0) + hist_idx = -hist_idx + RGN_HIST_SIZE / 2 - 1; + else + hist_idx = hist_idx + RGN_HIST_SIZE / 2; + //ж + bool isSame = isSameColor(hsvPattern, hsvPts[line][j], colorCmpParam); + if (true == isSame) + rgnHists[m].vldNum[hist_idx] ++; + rgnHists[m].totalNum[hist_idx] ++; + } + } + } + } + } + + + for (int m = 0; m < objNum; m++) + { + double normalizeHist[RGN_HIST_SIZE]; + //ɹһ + int totalNum=0, totalVldNum=0, upVldNum=0, dwnVldNum=0; + for (int j = 0; j < RGN_HIST_SIZE; j++) + { + totalNum += rgnHists[m].totalNum[j]; + totalVldNum += rgnHists[m].vldNum[j]; + + if (j < (RGN_HIST_SIZE / 2-2)) + dwnVldNum += rgnHists[m].vldNum[j]; + else if (j >= (RGN_HIST_SIZE / 2+2)) + upVldNum += rgnHists[m].vldNum[j]; + + if (rgnHists[m].totalNum[j] > 0) + normalizeHist[j] = (double)rgnHists[m].vldNum[j] / (double)rgnHists[m].totalNum[j]; + else + normalizeHist[j] = 0; + } + + SSG_peakOrienRgnInfo a_obj; + a_obj.centerPos = peakRgn[m].centerPos; + a_obj.objSize = peakRgn[m].objSize; + a_obj.pkRgnIdx = peakRgn[m].pkRgnIdx; + a_obj.pos2D = peakRgn[m].pos2D; + a_obj.orienFlag = 0; //δ֪ + //ƶȱȽ +#if 0 + double frontSim = cosSimilarity(frontColorTemplate, normalizeHist); + double frontSim_inv = cosSimilarityInv(frontColorTemplate, normalizeHist); + double backSim = cosSimilarity(backColorTemplate, normalizeHist); + double backSim_inv = cosSimilarityInv(backColorTemplate, normalizeHist); + if ((frontSim > frontSim_inv) && (frontSim > backSim) && (frontSim > backSim_inv) && (frontSim > 0.5)) + { + a_obj.orienFlag = 1; + } + else if ((frontSim_inv > frontSim) && (frontSim_inv > backSim) && (frontSim_inv > backSim_inv) && (frontSim_inv > 0.5)) + { + a_obj.orienFlag = 1; + a_obj.centerPos.z_yaw += 180; + if (a_obj.centerPos.z_yaw >= 360) + a_obj.centerPos.z_yaw = a_obj.centerPos.z_yaw - 360; + } + else if ((backSim > frontSim) && (backSim > frontSim_inv) && (backSim > backSim_inv) && (backSim > 0.5)) + { + a_obj.orienFlag = 2; + } + else if ((backSim_inv > frontSim) && (backSim_inv > frontSim_inv) && (backSim_inv > backSim) && (backSim_inv > 0.5)) + { + a_obj.orienFlag = 2; + a_obj.centerPos.z_yaw += 180; + if (a_obj.centerPos.z_yaw >= 360) + a_obj.centerPos.z_yaw = a_obj.centerPos.z_yaw - 360; + } +#else + double vldPtRatio = (double)totalVldNum / (double)totalNum; + bool isFront; + if (true == colorCmpParam.frontVldPtGreater) + isFront = vldPtRatio > colorCmpParam.FBVldPtRatioTh ? true : false; + else + isFront = vldPtRatio > colorCmpParam.FBVldPtRatioTh ? false : true; + + bool orienIsNormal; + if (true == isFront) + { + if (true == colorCmpParam.front_upVldPtGreater) + orienIsNormal = upVldNum > dwnVldNum ? true : false; + else + orienIsNormal = upVldNum > dwnVldNum ? false : true; + } + else + { + if (true == colorCmpParam.back_upVldPtGreater) + orienIsNormal = upVldNum > dwnVldNum ? true : false; + else + orienIsNormal = upVldNum > dwnVldNum ? false : true; + } + + if (true == isFront) + a_obj.orienFlag = 1; + else + a_obj.orienFlag = 2; + + if (false == orienIsNormal) + { + a_obj.centerPos.z_yaw += 180; + if (a_obj.centerPos.z_yaw >= 360) + a_obj.centerPos.z_yaw = a_obj.centerPos.z_yaw - 360; + } +#endif + objOps.push_back(a_obj); + } + } + +} + bool compareByXPos(const SSG_featureTree& a, const SSG_featureTree& b) { return (a.roi.left + a.roi.right) < (b.roi.left + b.roi.right); } diff --git a/sourceCode/SG_bagPositioning_Export.h b/sourceCode/SG_bagPositioning_Export.h index 07891c2..867a6e2 100644 --- a/sourceCode/SG_bagPositioning_Export.h +++ b/sourceCode/SG_bagPositioning_Export.h @@ -6,8 +6,9 @@ # define SG_APISHARED_EXPORT __declspec(dllimport) #endif -#define OUTPUT_DEBUG 0 +#define OUTPUT_DEBUG 1 #define BAG_ALGO_USE_CORNER_FEATURE 1 +#define RGN_HIST_SIZE 16 //ĿɫͳƵĿ #include "SG_baseDataType.h" #include @@ -27,6 +28,16 @@ typedef struct //SSG_polarScanParam polarScanParam; }SG_bagPositionParam; +typedef struct +{ + double hueTh; + double saturateTh; + double FBVldPtRatioTh; //Чɫı + bool frontVldPtGreater; //trueЧɫߵĵ棻falseЧɫߵĵǷ + bool front_upVldPtGreater;//trueЧɫߵĵ泯ϣfalseЧɫߵĵ泯 + bool back_upVldPtGreater; //trueЧɫߵĵǷ泯ϣfalseЧɫߵĵǷ泯 +}SSG_hsvCmpParam; + typedef struct { double jumpTh; //ֳйأΪ̵Ŀ׶Z @@ -34,7 +45,13 @@ typedef struct double baseHoleDist; //׶ľ }SSG_stackBaseParam; -SG_APISHARED_EXPORT void sg_lineDataR(SVzNL3DLaserLine* a_line, +//ݵƽ +SG_APISHARED_EXPORT void sg_lineDataR( + SVzNL3DLaserLine* a_line, + const double* camPoseR, + double groundH); +SG_APISHARED_EXPORT void sg_lineDataR_RGBD( + SVzNLXYZRGBDLaserLine* a_line, const double* camPoseR, double groundH); @@ -56,6 +73,23 @@ SG_APISHARED_EXPORT void sg_getBagPosition( const SSG_planeCalibPara poseCalibPara, std::vector& objOps); +SG_APISHARED_EXPORT void sg_getBagPositionAndOrientation( + SVzNLXYZRGBDLaserLine* laser3DPoints, + int lineNum, + //std::vector& all_vLineFeatures, + //std::vector>& noisePts, + const SG_bagPositionParam algoParam, + const SSG_planeCalibPara poseCalibPara, + const SSG_hsvCmpParam colorCmpParam, + const RGB rgbColorPattern, //ִӷɫ + const double frontColorTemplate[RGN_HIST_SIZE], + const double backColorTemplate[RGN_HIST_SIZE], + std::vector& objOps, +#if OUTPUT_DEBUG + std::vector>& bagPositionCloudPts, +#endif + int* errCode); + SG_APISHARED_EXPORT void sg_sideBagPosition( SVzNL3DLaserLine* laser3DPoints, int lineNum, diff --git a/sourceCode/SG_baseAlgo_Export.h b/sourceCode/SG_baseAlgo_Export.h index 552b2ec..32c2087 100644 --- a/sourceCode/SG_baseAlgo_Export.h +++ b/sourceCode/SG_baseAlgo_Export.h @@ -75,6 +75,14 @@ SG_APISHARED_EXPORT void sg_getLineLocalPeaks( std::vector< SSG_basicFeature1D>& localMax, std::vector< SSG_basicFeature1D>& localMin); +SG_APISHARED_EXPORT void sg_getFlatLineLocalPeaks_vector( + std::vector& lineData, + int lineIdx, + const double scaleWin, + const double minPkHeighth, + const double holeR, + std::vector< SSG_basicFeature1D>& localMax); + SG_APISHARED_EXPORT void sg_getLineDownJumps( SVzNL3DPosition* lineData, int dataSize, @@ -88,6 +96,13 @@ SG_APISHARED_EXPORT void sg_getFeatureGrowingTrees( std::vector& trees, SSG_treeGrowParam growParam); +SG_APISHARED_EXPORT void sg_lineFeaturesGrowing( + int lineIdx, + bool isLastLine, + std::vector& features, + std::vector& trees, + SSG_treeGrowParam growParam); + //ending SG_APISHARED_EXPORT void sg_getEndingGrowingTrees( std::vector& lineEndings, @@ -212,6 +227,10 @@ SG_APISHARED_EXPORT void sg_getLineRigthAngleFeature( SG_APISHARED_EXPORT SVzNL3DRangeD sg_getScanDataROI( SVzNL3DLaserLine* laser3DPoints, int lineNum); +//ɨROI: vecotrʽ +SG_APISHARED_EXPORT SVzNL3DRangeD sg_getScanDataROI_vector( + std::vector< std::vector>& scanLines +); //XYƽֱ SG_APISHARED_EXPORT void lineFitting( @@ -264,4 +283,18 @@ SG_APISHARED_EXPORT void eulerToRotationMatrixZYX(const SSG_EulerAngles& angles, SG_APISHARED_EXPORT void lineDataRT( SVzNL3DLaserLine* a_line, const double* camPoseR, - double groundH); \ No newline at end of file + double groundH); +SG_APISHARED_EXPORT void lineDataRT_vector( + std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH); +SG_APISHARED_EXPORT void lineDataRT_RGBD( + SVzNLXYZRGBDLaserLine* a_line, + const double* camPoseR, + double groundH); + +SG_APISHARED_EXPORT void sg_pointClustering( + std::vector< SVzNL3DPosition>& pts, + double clusterDist, + std::vector>& objClusters //result +); \ No newline at end of file diff --git a/sourceCode/SG_baseDataType.h b/sourceCode/SG_baseDataType.h index c2f44be..5cc63de 100644 --- a/sourceCode/SG_baseDataType.h +++ b/sourceCode/SG_baseDataType.h @@ -2,6 +2,7 @@ #include #include +#include #define PI 3.141592654 @@ -55,6 +56,18 @@ typedef struct double bottom; }SSG_ROIRectD; +struct HSV { + double h; // ɫ (0-360) + double s; // Ͷ (0-1) + double v; // (0-1) +}; + +struct RGB { + int r; + int g; + int b; +}; + #define LINE_FEATURE_NUM 16 #define LINE_FEATURE_UNDEF 0 #define LINE_FEATURE_L_JUMP_H2L 1 @@ -311,6 +324,15 @@ typedef struct SSG_6DOF centerPos; }SSG_peakRgnInfo; +typedef struct +{ + int pkRgnIdx; + SVzNLSizeD objSize; + SVzNL2DPoint pos2D; + SSG_6DOF centerPos; + int orienFlag; //0-δ֪1 - 棬 2- +}SSG_peakOrienRgnInfo; + typedef struct { SVzNL3DPoint sPt; diff --git a/sourceCode/SG_baseFunc.cpp b/sourceCode/SG_baseFunc.cpp index 103bf31..083f0ba 100644 --- a/sourceCode/SG_baseFunc.cpp +++ b/sourceCode/SG_baseFunc.cpp @@ -64,6 +64,66 @@ SVzNL3DRangeD sg_getScanDataROI( return roi; } +SVzNL3DRangeD sg_getScanDataROI_vector(std::vector< std::vector>& scanLines) +{ + SVzNL3DRangeD roi; + roi.xRange = { 0, -1 }; + roi.yRange = { 0, -1 }; + roi.zRange = { 0, -1 }; + + int lineNum = scanLines.size(); + for (int line = 0; line < lineNum; line++) + { + int nPositionCnt = scanLines[line].size(); + for (int i = 0; i < nPositionCnt; i++) + { + SVzNL3DPosition* pt3D = &scanLines[line][i]; + if (pt3D->pt3D.z < 1e-4) + continue; + + if (roi.xRange.max < roi.xRange.min) + { + roi.xRange.min = pt3D->pt3D.x; + roi.xRange.max = pt3D->pt3D.x; + } + else + { + if (roi.xRange.min > pt3D->pt3D.x) + roi.xRange.min = pt3D->pt3D.x; + if (roi.xRange.max < pt3D->pt3D.x) + roi.xRange.max = pt3D->pt3D.x; + } + //y + if (roi.yRange.max < roi.yRange.min) + { + roi.yRange.min = pt3D->pt3D.y; + roi.yRange.max = pt3D->pt3D.y; + } + else + { + if (roi.yRange.min > pt3D->pt3D.y) + roi.yRange.min = pt3D->pt3D.y; + if (roi.yRange.max < pt3D->pt3D.y) + roi.yRange.max = pt3D->pt3D.y; + } + //z + if (roi.zRange.max < roi.zRange.min) + { + roi.zRange.min = pt3D->pt3D.z; + roi.zRange.max = pt3D->pt3D.z; + } + else + { + if (roi.zRange.min > pt3D->pt3D.z) + roi.zRange.min = pt3D->pt3D.z; + if (roi.zRange.max < pt3D->pt3D.z) + roi.zRange.max = pt3D->pt3D.z; + } + } + } + return roi; +} + void lineFitting(std::vector< SVzNL3DPoint>& inliers, double* _k, double* _b) { //Сֱ߲ @@ -876,9 +936,10 @@ SSG_planeCalibPara sg_getPlaneCalibPara( zHist[histPos] ++; } } - //Ϊλۼ std::vector zSumHist; zSumHist.resize(zHistSize); + bool isSame = true; + //Ϊλۼ for (int i = 0; i < zHistSize; i++) { int sumValue = 0; @@ -888,6 +949,17 @@ SSG_planeCalibPara sg_getPlaneCalibPara( sumValue += zHist[j]; } zSumHist[i] = sumValue; + if (i > 0) + { + if (sumValue != zSumHist[i - 1]) + isSame = false; + } + } + if(true == isSame) + { + //ۼӣۼӣۼֵȣ + for (int i = 0; i < zHistSize; i++) + zSumHist[i] = zHist[i]; } //ѰҼֵ @@ -936,6 +1008,8 @@ SSG_planeCalibPara sg_getPlaneCalibPara( pkTop.push_back({pre_i, pre_data}); _state = 2; } + else if(i == (zHistSize-1)) + pkTop.push_back({ i, curr_data }); break; case 2: //½ if (z_diff > 0) // @@ -945,6 +1019,12 @@ SSG_planeCalibPara sg_getPlaneCalibPara( pkBtm.push_back({ pre_i, pre_data }); _state = 1; } + else if (i == (zHistSize - 1)) + { + int pkBtmIdx = pkBtm.size(); + pkBtmBackIndexing[i] = pkBtmIdx; + pkBtm.push_back({ i, curr_data }); + } break; default: _state = 0; @@ -1377,4 +1457,42 @@ void lineDataRT(SVzNL3DLaserLine* a_line, const double* camPoseR, double groundH a_line->p3DPosition[i].pt3D = a_pt; } return; +} +void lineDataRT_vector(std::vector< SVzNL3DPosition>& a_line, const double* camPoseR, double groundH) +{ + for (int i = 0; i < a_line.size(); i++) + { + SVzNL3DPoint a_pt = a_line[i].pt3D; + if (a_pt.z < 1e-4) + continue; + double x = a_pt.x * camPoseR[0] + a_pt.y * camPoseR[1] + a_pt.z * camPoseR[2]; + double y = a_pt.x * camPoseR[3] + a_pt.y * camPoseR[4] + a_pt.z * camPoseR[5]; + double z = a_pt.x * camPoseR[6] + a_pt.y * camPoseR[7] + a_pt.z * camPoseR[8]; + if ((groundH > 0) && (z > groundH)) //ȥ + z = 0; + a_pt.x = x; + a_pt.y = y; + a_pt.z = z; + a_line[i].pt3D = a_pt; + } + return; +} +void lineDataRT_RGBD(SVzNLXYZRGBDLaserLine* a_line, const double* camPoseR, double groundH) +{ + for (int i = 0; i < a_line->nPointCnt; i++) + { + SVzNLPointXYZRGBA a_pt = a_line->p3DPoint[i]; + if (a_pt.z < 1e-4) + continue; + double x = a_pt.x * camPoseR[0] + a_pt.y * camPoseR[1] + a_pt.z * camPoseR[2]; + double y = a_pt.x * camPoseR[3] + a_pt.y * camPoseR[4] + a_pt.z * camPoseR[5]; + double z = a_pt.x * camPoseR[6] + a_pt.y * camPoseR[7] + a_pt.z * camPoseR[8]; + if ((groundH > 0) && (z > groundH)) //ȥ + z = 0; + a_pt.x = x; + a_pt.y = y; + a_pt.z = z; + a_line->p3DPoint[i] = a_pt; + } + return; } \ No newline at end of file diff --git a/sourceCode/SG_featureGrow.cpp b/sourceCode/SG_featureGrow.cpp index 91730fe..a9724b4 100644 --- a/sourceCode/SG_featureGrow.cpp +++ b/sourceCode/SG_featureGrow.cpp @@ -407,6 +407,54 @@ void sg_getFeatureGrowingTrees( } } +void sg_lineFeaturesGrowing( + int lineIdx, + bool isLastLine, + std::vector& features, + std::vector& trees, + SSG_treeGrowParam growParam) +{ + if (features.size() > 0) + { + for (int j = 0, j_max = features.size(); j < j_max; j++) + { + SSG_basicFeature1D& a_feature = features[j]; + if (a_feature.jumpPos2D.x== 207) + int kkk = 1; + bool isMatched = _featureGrowing(a_feature, lineIdx, trees, growParam); + if (false == isMatched) + { + //µ + SSG_featureTree a_newTree; + a_newTree.treeNodes.push_back(a_feature); + a_newTree.treeState = TREE_STATE_ALIVE; + a_newTree.treeType = a_feature.featureType; + a_newTree.sLineIdx = lineIdx; + a_newTree.eLineIdx = lineIdx; + trees.push_back(a_newTree); + } + } + } + //ֹͣ,١ + //ڵΪ1Ƴ + int m_max = 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().jumpPos2D.x); + if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) || + (true == isLastLine)) + { + trees[m].treeState = TREE_STATE_DEAD; + bool isValid = _invalidateVSlopeTrees(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 52d2ab4..72933bd 100644 --- a/sourceCode/SG_lineFeature.cpp +++ b/sourceCode/SG_lineFeature.cpp @@ -149,7 +149,10 @@ if (i == 370) } //˳Ⱥ㣺z޷ -void sg_lineDataRemoveOutlier_changeOriginData(SVzNL3DPosition* lineData, int dataSize, SSG_outlierFilterParam filterParam) +void sg_lineDataRemoveOutlier_changeOriginData( + SVzNL3DPosition* lineData, + int dataSize, + SSG_outlierFilterParam filterParam) { std::vector< SSG_RUN> continueRuns; SSG_RUN a_run = { 0, -1, 0 }; //startIdx, len, lastIdx @@ -1539,6 +1542,804 @@ void sg_getLineLocalPeaks( return; } +#if 0 +typedef struct +{ + double holeLen; + double holeCenterY; + double holeCenterX; + int holeSIdx; + int holeEIdx; +}_HoleInfo; + +typedef struct +{ + double maxZ; + double minZ; + double pk_y; + double pk_x; + int pkIdx; +}_Feature; +//ԵƽֱѰͻ𡣲÷ֶηѰҡ +void sg_getFlatLineLocalPeaks_vector( + std::vector& lineData, + int lineIdx, + const double scaleWin, + const double minPkHeighth, + const double holeR, + std::vector< SSG_basicFeature1D>& localMax) +{ + int dataSize = lineData.size(); + if (dataSize < 2) + return; + + double maxZ = 0; + double subScale = scaleWin / 3.0; + std::vector<_Feature> subFeatures; + int maxIdx = -1; + int minIdx = -1; + bool startVld = false; + double startY = 0; + int preVldPt = 0; + int holePtNum = 0; + _HoleInfo a_hole = { 0.0, 0.0, 0.0, -1, -1 }; + for (int i = 0; i < dataSize; i++) + { + lineData[i].nPointIdx = 0; + if ((lineIdx == 1219) && (i == 2722)) + int kkk = 1; + + if (lineData[i].pt3D.z < 1e-4) + { + if (true == startVld) + holePtNum++; + } + else + { + if (maxZ < lineData[i].pt3D.z) + maxZ = lineData[i].pt3D.z; + + if (false == startVld) + { + startVld = true; + startY = lineData[i].pt3D.y; + } + //׶ + if ((holePtNum > 0) && (preVldPt >= 0)) + { + double holeLen = lineData[i].pt3D.y - lineData[preVldPt].pt3D.y; + if (holeLen > holeR / 3) + { + _HoleInfo currHole; + currHole.holeLen = holeLen; + currHole.holeCenterY = (lineData[i].pt3D.y + lineData[preVldPt].pt3D.y) / 2; + currHole.holeCenterX = (lineData[i].pt3D.x + lineData[preVldPt].pt3D.x) / 2; + currHole.holeSIdx = preVldPt; + currHole.holeEIdx = i; + + if (a_hole.holeSIdx >= 0) + { + //׶ǷԺϲܺϲȡĿ׶ + double hole_dist = currHole.holeCenterY - a_hole.holeCenterY; + if (hole_dist < holeR * 2) + { + a_hole.holeEIdx = currHole.holeEIdx; + a_hole.holeLen = lineData[a_hole.holeEIdx].pt3D.y - lineData[a_hole.holeSIdx].pt3D.y; + a_hole.holeCenterY = (lineData[a_hole.holeEIdx].pt3D.y + lineData[a_hole.holeSIdx].pt3D.y) / 2; + } + else if (a_hole.holeLen < currHole.holeLen) + a_hole = currHole; + + } + else + a_hole = currHole; + } + //¼׶ + holePtNum = 0; + } + + double diffY = lineData[i].pt3D.y - startY; + if (diffY >= subScale) //ֶμ + { + if (a_hole.holeSIdx >= 0) + { + _Feature a_subFeature; + a_subFeature.minZ = -1; + a_subFeature.maxZ = -1; + a_subFeature.pk_y = a_hole.holeCenterY; + a_subFeature.pk_x = a_hole.holeCenterX; + a_subFeature.pkIdx = (a_hole.holeSIdx + a_hole.holeEIdx) / 2; + subFeatures.push_back(a_subFeature); + } + else if (maxIdx >= 0) + { + double depth = lineData[maxIdx].pt3D.z - lineData[minIdx].pt3D.z; + if (depth > minPkHeighth / 2) + { + _Feature a_subFeature; + a_subFeature.minZ = lineData[minIdx].pt3D.z; + a_subFeature.maxZ = lineData[maxIdx].pt3D.z; + a_subFeature.pk_x = lineData[maxIdx].pt3D.x; + a_subFeature.pk_y = lineData[maxIdx].pt3D.y; + a_subFeature.pkIdx = maxIdx; + subFeatures.push_back(a_subFeature); + } + } + //ֶ¿ʼ + maxIdx = -1; + minIdx = -1; + startY = lineData[i].pt3D.y; + a_hole = { 0.0, 0.0, 0.0, -1, -1 }; + } + + if (maxIdx < 0) + maxIdx = i; + else + { + if (lineData[maxIdx].pt3D.z < lineData[i].pt3D.z) + maxIdx = i; + } + if (minIdx < 0) + minIdx = i; + else + { + if (lineData[minIdx].pt3D.z > lineData[i].pt3D.z) + minIdx = i; + } + + //һ + if (i == dataSize - 1) + { + if (a_hole.holeSIdx >= 0) + { + _Feature a_subFeature; + a_subFeature.minZ = -1; + a_subFeature.maxZ = -1; + a_subFeature.pk_x = a_hole.holeCenterX; + a_subFeature.pk_y = a_hole.holeCenterY; + a_subFeature.pkIdx = (a_hole.holeSIdx + a_hole.holeEIdx) / 2; + subFeatures.push_back(a_subFeature); + } + else if (maxIdx >= 0) + { + _Feature a_subFeature; + a_subFeature.minZ = lineData[minIdx].pt3D.z; + a_subFeature.maxZ = lineData[maxIdx].pt3D.z; + a_subFeature.pk_x = lineData[maxIdx].pt3D.x; + a_subFeature.pk_y = lineData[maxIdx].pt3D.y; + a_subFeature.pkIdx = maxIdx; + subFeatures.push_back(a_subFeature); + } + } + preVldPt = i; + } + } + + for (int i = 0, i_max = subFeatures.size(); i < i_max; i++) + { + _Feature* currFeature = &subFeatures[i]; + if (currFeature->pkIdx < 0) + continue; + if (currFeature->maxZ < 0) // ׶ + { + bool isCombined = false; + if (i < i_max - 1) //ϲ + { + _Feature* nxtFeature = &subFeatures[i + 1]; + //׶ǷԺϲܺϲȡĿ׶ + double hole_dist = nxtFeature->pk_y - currFeature->pk_y; + if (hole_dist < holeR*2) //ϲ + { + if (nxtFeature->maxZ < 0) + { + nxtFeature->pkIdx = (currFeature->pkIdx + nxtFeature->pkIdx) / 2; + nxtFeature->pk_x = (currFeature->pk_x + nxtFeature->pk_x) / 2; + nxtFeature->pk_y = (currFeature->pk_y + nxtFeature->pk_y) / 2; + } + currFeature->pkIdx = -1; //ϲ + isCombined = true; + } + } + if (false == isCombined) + { + // + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_PEAK_TOP; + a_feature.jumpPos2D = { lineIdx, currFeature->pkIdx }; + a_feature.jumpPos.x = currFeature->pk_x; + a_feature.jumpPos.y = currFeature->pk_y; + a_feature.jumpPos.z = maxZ; + localMax.push_back(a_feature); + lineData[currFeature->pkIdx].nPointIdx = LINE_FEATURE_PEAK_TOP; //nPointIdxתʹ + lineData[currFeature->pkIdx].pt3D = a_feature.jumpPos; + } + } + else + { + bool isBigger_1 = true; //ǰһ + bool isBigger_2 = true; //Ⱥһ + double minZ = currFeature->minZ; + + if (i > 0) + { + _Feature* preFeature = &subFeatures[i - 1]; + if (preFeature->maxZ < 0) + { + if(preFeature->pkIdx >= 0) + isBigger_1 = false; + } + else + { + if (currFeature->minZ > preFeature->minZ) + minZ = preFeature->minZ; + if (currFeature->maxZ < preFeature->maxZ) + isBigger_1 = false; + } + } + if (i < i_max - 1) + { + _Feature* nxtFeature = &subFeatures[i + 1]; + if (nxtFeature->maxZ < 0) + { + double hole_dist = nxtFeature->pk_y - currFeature->pk_y; + if (hole_dist >= holeR * 2) + isBigger_2 = false; + else + nxtFeature->pkIdx = -1; //Ŀױϲ + } + else + { + if (currFeature->minZ > nxtFeature->minZ) + minZ = nxtFeature->minZ; + if (currFeature->maxZ < nxtFeature->maxZ) + isBigger_2 = false; + } + } + if ((true == isBigger_1) && (true == isBigger_2)) + { + double z_diff = currFeature->maxZ - minZ; + if (z_diff > minPkHeighth) //ϸͻ + { + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_PEAK_TOP; + a_feature.jumpPos2D = { lineIdx, currFeature->pkIdx }; + a_feature.jumpPos = lineData[currFeature->pkIdx].pt3D; + localMax.push_back(a_feature); + lineData[currFeature->pkIdx].nPointIdx = LINE_FEATURE_PEAK_TOP; //nPointIdxתʹ + } + } + } + } + return; +} +#else +//ԵƽֱѰͻ +//бѰңѰ䡢б¡Gap +typedef struct +{ + int type; + int ptIdxS; + int ptIdxE; + int sIdx; + int eIdx; + SVzNL3DPoint start; + SVzNL3DPoint end; + double delta_y; + double delta_z; + double k; +}_FeatureInfo; + +int _findSegMaxZPt( + std::vector& lineData, + int searchStart, + bool searchToHead, + double searchWin, + double* meanZ, + bool* foundJump) +{ + int dataSize = lineData.size(); + double start_y = lineData[searchStart].pt3D.y; + double start_z = lineData[searchStart].pt3D.z; + if (start_z < 1e-4) + return -1; + + double max_z = start_z; + int max_idx = searchStart; + double sum_z = max_z; + int sum_num = 1; + int step = searchToHead == true ? -1 : 1; + int idx = searchStart; + while (1) + { + idx += step; + if ((idx <= 0) || (idx >= dataSize)) + break; + + if (lineData[idx].nPointIdx == LINE_FEATURE_NUM) + { + *foundJump = true; + break; + } + double pt_y = lineData[idx].pt3D.y; + double pt_z = lineData[idx].pt3D.z; + if (pt_z > 1e-4) + { + double dist = abs(pt_y - start_y); + if (dist > searchWin) + { + *meanZ = sum_z / sum_num; + return max_idx; + } + else + { + sum_z += pt_z; + sum_num++; + if (max_z < pt_z) + { + max_z = pt_z; + max_idx = idx; + } + } + + } + } + return -1; +} + +void sg_getFlatLineLocalPeaks_vector( + std::vector& lineData, + int lineIdx, + const double scaleWin, + const double minPkHeighth, + const double holeR, + std::vector< SSG_basicFeature1D>& localMax) +{ + int dataSize = lineData.size(); + if (dataSize < 2) + return; + + //٣г + //ĵȥյ + double slopeZDeltaTH = 0.2; //slopez仯ޣڴΪslope + int dwnSample = 1; + int sampleNum = dataSize / dwnSample; + std::vector sampleData; + for (int i = 0; i < sampleNum; i++) + { + SVzNL3DPosition a_sample = lineData[i * dwnSample]; + a_sample.nPointIdx = i * dwnSample; + if(a_sample.pt3D.z > 1e-4) + sampleData.push_back(a_sample); + } + + sampleNum = sampleData.size(); + std::vector< _FeatureInfo> allSubFeatures; + allSubFeatures.resize(sampleNum); + for (int i = 0; i < sampleNum; i++) //ʼ + allSubFeatures[i].type = LINE_FEATURE_UNDEF; + + int segEnd = 0; + for (int i = 0; i < sampleNum-1; i++) + { + SVzNL3DPosition* samplePtr = &sampleData[i]; + if ((lineIdx == 0) && (i == 2290)) + int kkk = 1; + //ǷΪgap + SVzNL3DPosition* nxtSamplePtr = &sampleData[i+1]; + //double d_delta = sqrt(pow(nxtSamplePtr->pt3D.y - samplePtr->pt3D.y, 2) + pow(nxtSamplePtr->pt3D.z - samplePtr->pt3D.z, 2)); + double y_delta = abs(nxtSamplePtr->pt3D.y - samplePtr->pt3D.y); + double z_delta = nxtSamplePtr->pt3D.z - samplePtr->pt3D.z; + if ((y_delta > holeR / 2) || (abs(z_delta) > minPkHeighth)) + { + _FeatureInfo a_subFeature; + a_subFeature.delta_y = y_delta; + a_subFeature.delta_z = z_delta; + a_subFeature.k = z_delta / y_delta; + a_subFeature.ptIdxS = i; + a_subFeature.ptIdxE = i+1; + a_subFeature.sIdx = sampleData[i].nPointIdx; + a_subFeature.eIdx = sampleData[i + 1].nPointIdx; + a_subFeature.start = samplePtr->pt3D; + a_subFeature.end = nxtSamplePtr->pt3D; + if(z_delta > 0) + a_subFeature.type = LINE_FEATURE_L_JUMP_L2H; + else + a_subFeature.type = LINE_FEATURE_L_JUMP_H2L; + allSubFeatures[i] = a_subFeature; + segEnd = i + 1; + } + else + { + //Ѱҳ߶ȶ˵ + for (int j = segEnd; j < sampleNum; j++) + { + SVzNL3DPosition* postSamplePtr = &sampleData[j]; + double deltaY = abs(postSamplePtr->pt3D.y - samplePtr->pt3D.y); + if (deltaY >= scaleWin) + { + double deltaZ = postSamplePtr->pt3D.z - samplePtr->pt3D.z; + if (abs(deltaZ) > slopeZDeltaTH) + int kkk = 1; + _FeatureInfo a_subFeature; + a_subFeature.delta_y = deltaY; + a_subFeature.delta_z = deltaZ; + a_subFeature.k = deltaZ / deltaY; + a_subFeature.ptIdxS = i; + a_subFeature.ptIdxE = j; + a_subFeature.sIdx = sampleData[i].nPointIdx; + a_subFeature.eIdx = sampleData[j].nPointIdx; + a_subFeature.start = samplePtr->pt3D; + a_subFeature.end = postSamplePtr->pt3D; + if ( (abs(a_subFeature.k) > 0.5) && (abs(deltaZ) > slopeZDeltaTH)) + { + if (deltaZ > 0) + a_subFeature.type = LINE_FEATURE_L_SLOPE_L2H; + else + a_subFeature.type = LINE_FEATURE_L_SLOPE_H2L; + } + else + a_subFeature.type = LINE_FEATURE_UNDEF; + allSubFeatures[i] = a_subFeature; + segEnd = j; + break; + } + } + } + } + + + //JUMP, ڵLINE_FEATURE_L_SLOPE_L2HЧ + for (int i = 0; i < sampleNum; i++) + { + if (allSubFeatures[i].type == LINE_FEATURE_L_JUMP_L2H) + { + //ڵLINE_FEATURE_L_SLOPE_L2HЧ + for (int j = i + 1; j < sampleNum; j++) + { + if (allSubFeatures[j].type == LINE_FEATURE_L_SLOPE_L2H) + allSubFeatures[j].type = LINE_FEATURE_UNDEF; + else + break; + } + } + else if (allSubFeatures[i].type == LINE_FEATURE_L_JUMP_H2L) + { + //ڵLINE_FEATURE_L_SLOPE_L2HЧ + for (int j = i - 1; j >= 0; j--) + { + if (allSubFeatures[j].type == LINE_FEATURE_L_SLOPE_H2L) + allSubFeatures[j].type = LINE_FEATURE_UNDEF; + else + break; + } + } + } +#if 0 + std::vector< _FeatureInfo> subFeatures; + for (int i = 0; i < sampleNum; i++) + { + if (allSubFeatures[i].type != LINE_FEATURE_UNDEF) + subFeatures.push_back(allSubFeatures[i]); + } + _FeatureInfo a_nullFeature; + memset(&a_nullFeature, 0, sizeof(_FeatureInfo)); + subFeatures.push_back(a_nullFeature); //ڽβһգдʱȷһ + int subfeatureSize = subFeatures.size(); +#endif + //ϸsubFeatureѡ + int pre_type = LINE_FEATURE_UNDEF; + _FeatureInfo a_vldFeature; + std::vector< _FeatureInfo> vldFeatures; + for (int i = 0; i < sampleNum; i++) + { + if (pre_type == LINE_FEATURE_UNDEF) + { + if ( (allSubFeatures[i].type == LINE_FEATURE_L_JUMP_H2L) || + (allSubFeatures[i].type == LINE_FEATURE_L_JUMP_L2H)) + { + vldFeatures.push_back(allSubFeatures[i]); + pre_type = LINE_FEATURE_UNDEF; + } + else + { + a_vldFeature = allSubFeatures[i]; + pre_type = allSubFeatures[i].type; + } + } + else + { + if (allSubFeatures[i].type == LINE_FEATURE_UNDEF) + { + vldFeatures.push_back(a_vldFeature); + } + else + { + if (((a_vldFeature.k < 0) && (allSubFeatures[i].k < 0)) || + ((a_vldFeature.k > 0) && (allSubFeatures[i].k > 0))) + { + if (abs(a_vldFeature.k) < abs(allSubFeatures[i].k)) + a_vldFeature = allSubFeatures[i]; + } + else + { + vldFeatures.push_back(a_vldFeature); + a_vldFeature = allSubFeatures[i]; + } + } + pre_type = allSubFeatures[i].type; + } + } + + //ԣ + //ȶJump + std::vector featureBuffer; + featureBuffer.resize(vldFeatures.size()); + for (int i = 0; i < vldFeatures.size(); i++) //ʼ + featureBuffer[i].featureType = LINE_FEATURE_UNDEF; + for (int i = 0, i_max = vldFeatures.size(); i < i_max; i++) + { + if (vldFeatures[i].type == LINE_FEATURE_L_JUMP_H2L) + { + //жzķ + if (abs(vldFeatures[i].delta_z) < minPkHeighth) //ûƥJUMP + { + // + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_PEAK_TOP; + int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2; + a_feature.jumpPos2D = { lineIdx, centerIdx }; + a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2; + a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2; + a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2; + int start = vldFeatures[i].sIdx; + int end = vldFeatures[i].eIdx; + //ƥJUMP + if (i < i_max - 1) + { + if ((vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_H2L) || + (vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_L2H)) + { + //жϾ + double dist = abs(vldFeatures[i + 1].start.y - vldFeatures[i].end.y); + if (dist < holeR) //ϲ + { + centerIdx = (vldFeatures[i].sIdx + vldFeatures[i+1].eIdx) / 2; + a_feature.jumpPos2D = { lineIdx, centerIdx }; + a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i+1].end.x) / 2; + a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i+1].end.y) / 2; + a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i+1].end.z) / 2; + vldFeatures[i + 1].type = LINE_FEATURE_UNDEF; + end = vldFeatures[i+1].eIdx; + } + } + } + featureBuffer[i] = a_feature; + lineData[start].nPointIdx = LINE_FEATURE_NUM; //յʶʹLINE_FEATURE_NUMʶ + lineData[end].nPointIdx = LINE_FEATURE_NUM; + } + else //½ֻƥûJUMP + { + // + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_PEAK_TOP; + int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2; + a_feature.jumpPos2D = { lineIdx, centerIdx }; + a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2; + a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2; + a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2; + int start = vldFeatures[i].sIdx; + int end = vldFeatures[i].eIdx; + if (i < i_max - 1) + { + if ((vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_H2L) || + (vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_L2H)) + { + if (abs(vldFeatures[i + 1].delta_z) < minPkHeighth) //ֻƥû + { + //жϾ + double dist = abs(vldFeatures[i + 1].start.y - vldFeatures[i].end.y); + if (dist < holeR) //ϲ + { + centerIdx = (vldFeatures[i].sIdx + vldFeatures[i + 1].eIdx) / 2; + a_feature.jumpPos2D = { lineIdx, centerIdx }; + a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i + 1].end.x) / 2; + a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i + 1].end.y) / 2; + a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i + 1].end.z) / 2; + vldFeatures[i + 1].type = LINE_FEATURE_UNDEF; + end = vldFeatures[i + 1].eIdx; + } + } + } + } + featureBuffer[i] = a_feature; + lineData[start].nPointIdx = LINE_FEATURE_NUM; //յʶʹLINE_FEATURE_NUMʶ + lineData[end].nPointIdx = LINE_FEATURE_NUM; + } + } + else if (vldFeatures[i].type == LINE_FEATURE_L_JUMP_L2H) + { + //жzķ + if (abs(vldFeatures[i].delta_z) < minPkHeighth) //ûƥJUMP + { + //ֱƥJUMP + // + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_PEAK_TOP; + int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2; + a_feature.jumpPos2D = { lineIdx, centerIdx }; + a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2; + a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2; + a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2; + int start = vldFeatures[i].sIdx; + int end = vldFeatures[i].eIdx; + //ƥJUMP + if (i < i_max - 1) + { + if ((vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_H2L) || + (vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_L2H)) + { + //жϾ + double dist = abs(vldFeatures[i + 1].start.y - vldFeatures[i].end.y); + if (dist < holeR) //ϲ + { + centerIdx = (vldFeatures[i].sIdx + vldFeatures[i + 1].eIdx) / 2; + a_feature.jumpPos2D = { lineIdx, centerIdx }; + a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i + 1].end.x) / 2; + a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i + 1].end.y) / 2; + a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i + 1].end.z) / 2; + vldFeatures[i + 1].type = LINE_FEATURE_UNDEF; + end = vldFeatures[i + 1].eIdx; + } + } + } + featureBuffer[i] = a_feature; + lineData[start].nPointIdx = LINE_FEATURE_NUM; //յʶʹLINE_FEATURE_NUMʶ + lineData[end].nPointIdx = LINE_FEATURE_NUM; + } + else //ģƥ½JUMPûJUMPSLOPE + { + // + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_PEAK_TOP; + int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2; + a_feature.jumpPos2D = { lineIdx, centerIdx }; + a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2; + a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2; + a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2; + int start = vldFeatures[i].sIdx; + int end = vldFeatures[i].eIdx; + if (i < i_max - 1) + { + if ((vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_H2L) || + ((vldFeatures[i + 1].type == LINE_FEATURE_L_JUMP_L2H) && (abs(vldFeatures[i+1].delta_z) < minPkHeighth)) || + (vldFeatures[i + 1].type == LINE_FEATURE_L_SLOPE_H2L)) + { + //жϾ + double dist = abs(vldFeatures[i + 1].start.y - vldFeatures[i].end.y); + if (dist < holeR) //ϲ + { + centerIdx = (vldFeatures[i].sIdx + vldFeatures[i + 1].eIdx) / 2; + a_feature.jumpPos2D = { lineIdx, centerIdx }; + a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i + 1].end.x) / 2; + a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i + 1].end.y) / 2; + a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i + 1].end.z) / 2; + vldFeatures[i + 1].type = LINE_FEATURE_UNDEF; + end = vldFeatures[i + 1].eIdx; + } + + } + } + featureBuffer[i] = a_feature; + lineData[start].nPointIdx = LINE_FEATURE_NUM; //յʶʹLINE_FEATURE_NUMʶ + lineData[end].nPointIdx = LINE_FEATURE_NUM; + } + } + } + //SLOPE + for (int i = 0, i_max = vldFeatures.size(); i < i_max; i++) + { + if (vldFeatures[i].type == LINE_FEATURE_L_SLOPE_L2H) + { + if (i < i_max - 1) + { + double dist = abs(vldFeatures[i + 1].start.y - vldFeatures[i].end.y); + if ((dist < holeR) && (vldFeatures[i + 1].type == LINE_FEATURE_L_SLOPE_H2L)) + { + // + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_PEAK_TOP; + int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i + 1].eIdx) / 2; + a_feature.jumpPos2D = { lineIdx, centerIdx }; + a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i + 1].end.x) / 2; + a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i + 1].end.y) / 2; + a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i + 1].end.z) / 2; + vldFeatures[i + 1].type = LINE_FEATURE_UNDEF; + featureBuffer[i] = a_feature; + } + else + { + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_PEAK_TOP; +#if 0 + int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2; + a_feature.jumpPos2D = { lineIdx, centerIdx }; + a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2; + a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2; + a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2; +#endif + //HoleRΧѰPeak + double mean_z = -1; + bool foundJump = false; + int pkIdx = _findSegMaxZPt(lineData, vldFeatures[i].eIdx, false, holeR*1.5, &mean_z, &foundJump); + //ѰǷJump + double meanZ_inv = -1; + bool foundJump_inv = false; + int pkIdx_inv = _findSegMaxZPt(lineData, vldFeatures[i].sIdx, true, holeR * 1.5, &meanZ_inv, &foundJump_inv); + if ( (pkIdx >= 0)&&(false == foundJump_inv)) + { + double mean_depth = mean_z - vldFeatures[i].start.z; + if (pkIdx_inv >= 0) + mean_depth = mean_z - meanZ_inv; + if (mean_depth >= slopeZDeltaTH/2) + { + a_feature.jumpPos2D = { lineIdx, pkIdx }; + a_feature.jumpPos = lineData[pkIdx].pt3D; + featureBuffer[i] = a_feature; + } + } + + } + } + } + else if (vldFeatures[i].type == LINE_FEATURE_L_SLOPE_H2L) + { + //HoleRΧѰPeak + SSG_basicFeature1D a_feature; + a_feature.featureType = LINE_FEATURE_PEAK_TOP; +#if 0 + int centerIdx = (vldFeatures[i].sIdx + vldFeatures[i].eIdx) / 2; + a_feature.jumpPos2D = { lineIdx, centerIdx }; + a_feature.jumpPos.x = (vldFeatures[i].start.x + vldFeatures[i].end.x) / 2; + a_feature.jumpPos.y = (vldFeatures[i].start.y + vldFeatures[i].end.y) / 2; + a_feature.jumpPos.z = (vldFeatures[i].start.z + vldFeatures[i].end.z) / 2; +#endif + //HoleRΧѰPeak + double mean_z = -1; + bool foundJump = false; + int pkIdx = _findSegMaxZPt(lineData, vldFeatures[i].sIdx, true, holeR*1.5, &mean_z, &foundJump); + //ѰǷJump + double meanZ_inv = -1; + bool foundJump_inv = false; + int pkIdx_inv = _findSegMaxZPt(lineData, vldFeatures[i].eIdx, false, holeR * 1.5, &meanZ_inv, &foundJump_inv); + if ((pkIdx >= 0) && (false == foundJump_inv)) + { + double mean_depth = mean_z - vldFeatures[i].start.z; + if (pkIdx_inv >= 0) + mean_depth = mean_z - meanZ_inv; + if (mean_depth >= slopeZDeltaTH/2) + { + a_feature.jumpPos2D = { lineIdx, pkIdx }; + a_feature.jumpPos = lineData[pkIdx].pt3D; + featureBuffer[i] = a_feature; + } + } + } + + } + for (int i = 0, i_max = vldFeatures.size(); i < i_max; i++) + { + if (featureBuffer[i].featureType != LINE_FEATURE_UNDEF) + { + localMax.push_back(featureBuffer[i]); + if (lineData[featureBuffer[i].jumpPos2D.y].pt3D.z < 1e-4) + lineData[featureBuffer[i].jumpPos2D.y].pt3D = featureBuffer[i].jumpPos; + lineData[featureBuffer[i].jumpPos2D.y].nPointIdx = LINE_FEATURE_PEAK_TOP; //nPointIdxתʹ + } + } + return; +} +#endif + /// /// ȡϵ㣨Zֵ䣩 ///