添加了拆包算法中编织袋正反识别和朝向识别算法，以及相应的测试程序
This commit is contained in:
parent
ae46895573
commit
497b98e2c3
@ -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
|
||||
|
||||
@ -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<SVzNL3DPoint>& objOps)
|
||||
float lineV, int maxTimeStamp, int clockPerSecond, std::vector<std::vector<SVzNL3DPoint>>& 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<SVzNL3DPoint>& 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<SVzNL3DPoint>& 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<std::vector< SVzNL3DPosition>>& dataLines, std::vector<SVzNL3DPoint>& 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<std::vector< SVzNL3DPosition>>& dataLines,
|
||||
std::vector<std::vector<SVzNL3DPoint>>& 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<std::vector< SVzNL3DPositi
|
||||
}
|
||||
if (objOps.size() > 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<SVzNL3DPoint>& 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<SVzNL3DPoint>& nodeOps)
|
||||
void _genXOYProjectionImage_nodePos(cv::String& fileName, SVzNL3DLaserLine* scanData, int lineNum, std::vector<std::vector<SVzNL3DPoint>>& nodeOps)
|
||||
{
|
||||
//统计X和Y的范围
|
||||
std::vector<std::vector< SVzNL3DPosition>> scan_lines;
|
||||
@ -911,12 +919,160 @@ SSG_planeCalibPara _readCalibPara(char* fileName)
|
||||
return planePara;
|
||||
}
|
||||
|
||||
std::vector<SVzNL3DPoint> _wdReadLMIData_TXT(char* fileName)
|
||||
{
|
||||
std::ifstream inputFile(fileName);
|
||||
std::string linedata;
|
||||
|
||||
std::vector<SVzNL3DPoint> 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<SVzNL3DPoint> _wdReadGuangZiData_TXT(char* fileName, double tick_scale, double offset)
|
||||
{
|
||||
std::ifstream inputFile(fileName);
|
||||
std::string linedata;
|
||||
|
||||
std::vector<SVzNL3DPoint> 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<SVzNL3DPoint>& ptList, int* vldLineNum)
|
||||
{
|
||||
|
||||
std::vector<std::vector< SVzNL3DPoint>> 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<SVzNL3DPoint> 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<SVzNL3DPoint> 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<SVzNL3DPoint> nodePos;
|
||||
std::vector<std::vector<SVzNL3DPoint>> nodePos;
|
||||
sg_getSieveNodes(
|
||||
laser3DPoints,
|
||||
lineNum,
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -166,6 +166,7 @@
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="..\sourceCode\SG_baseFunc.cpp" />
|
||||
<ClCompile Include="..\sourceCode\SG_clustering.cpp" />
|
||||
<ClCompile Include="..\sourceCode\SG_featureGrow.cpp" />
|
||||
<ClCompile Include="..\sourceCode\SG_lineFeature.cpp" />
|
||||
<ClCompile Include="..\sourceCode\SG_regionGrow.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; // 未定义，可以是任何值，通常设为0或NaN
|
||||
}
|
||||
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<SSG_lineFeature>& all_vLineFeatures,
|
||||
//std::vector<std::vector<int>>& 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<SSG_peakOrienRgnInfo>& objOps,
|
||||
#if OUTPUT_DEBUG
|
||||
std::vector<std::vector< SVzNL3DPosition>>& bagPositionCloudPts,
|
||||
#endif
|
||||
int* errCode)
|
||||
{
|
||||
*errCode = 0;
|
||||
if (lineNum == 0)
|
||||
{
|
||||
*errCode = SG_ERR_3D_DATA_INVLD;
|
||||
return;
|
||||
}
|
||||
//检查是否为栅格格式
|
||||
//将RGBD点云分离出对应的RGB和点云，RGB转成HSV
|
||||
std::vector<SVzNL3DLaserLine> cloudPts;
|
||||
std::vector<std::vector<HSV>> hsvPts;
|
||||
bool dataValid = true;
|
||||
int linePtNum = laser3DPoints[0].nPointCnt;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
if (laser3DPoints[line].nPointCnt > 0)
|
||||
{
|
||||
std::vector<HSV> 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<SSG_peakRgnInfo> 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<double> y_th;
|
||||
y_th.resize(objNum);
|
||||
std::vector<double> x_th;
|
||||
x_th.resize(objNum);
|
||||
std::vector<double> 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<SSG_ROIRectD> 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);
|
||||
}
|
||||
|
||||
@ -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 <vector>
|
||||
@ -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<SSG_peakRgnInfo>& objOps);
|
||||
|
||||
SG_APISHARED_EXPORT void sg_getBagPositionAndOrientation(
|
||||
SVzNLXYZRGBDLaserLine* laser3DPoints,
|
||||
int lineNum,
|
||||
//std::vector<SSG_lineFeature>& all_vLineFeatures,
|
||||
//std::vector<std::vector<int>>& 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<SSG_peakOrienRgnInfo>& objOps,
|
||||
#if OUTPUT_DEBUG
|
||||
std::vector<std::vector< SVzNL3DPosition>>& bagPositionCloudPts,
|
||||
#endif
|
||||
int* errCode);
|
||||
|
||||
SG_APISHARED_EXPORT void sg_sideBagPosition(
|
||||
SVzNL3DLaserLine* laser3DPoints,
|
||||
int lineNum,
|
||||
|
||||
@ -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<SVzNL3DPosition>& 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<SSG_featureTree>& trees,
|
||||
SSG_treeGrowParam growParam);
|
||||
|
||||
SG_APISHARED_EXPORT void sg_lineFeaturesGrowing(
|
||||
int lineIdx,
|
||||
bool isLastLine,
|
||||
std::vector<SSG_basicFeature1D>& features,
|
||||
std::vector<SSG_featureTree>& trees,
|
||||
SSG_treeGrowParam growParam);
|
||||
|
||||
//对ending进行生长
|
||||
SG_APISHARED_EXPORT void sg_getEndingGrowingTrees(
|
||||
std::vector<SSG_2DValueI>& 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<SVzNL3DPosition>>& 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);
|
||||
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<std::vector< SVzNL3DPosition>>& objClusters //result
|
||||
);
|
||||
@ -2,6 +2,7 @@
|
||||
|
||||
#include <VZNL_Types.h>
|
||||
#include <vector>
|
||||
#include <SG_errCode.h>
|
||||
|
||||
#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;
|
||||
|
||||
@ -64,6 +64,66 @@ SVzNL3DRangeD sg_getScanDataROI(
|
||||
return roi;
|
||||
}
|
||||
|
||||
SVzNL3DRangeD sg_getScanDataROI_vector(std::vector< std::vector<SVzNL3DPosition>>& 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<int> 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;
|
||||
}
|
||||
@ -407,6 +407,54 @@ void sg_getFeatureGrowingTrees(
|
||||
}
|
||||
}
|
||||
|
||||
void sg_lineFeaturesGrowing(
|
||||
int lineIdx,
|
||||
bool isLastLine,
|
||||
std::vector<SSG_basicFeature1D>& features,
|
||||
std::vector<SSG_featureTree>& 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<SSG_2DValueI>& lineEndings,
|
||||
SVzNL3DLaserLine* laser3DPoints,
|
||||
|
||||
@ -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<SVzNL3DPosition>& 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<SVzNL3DPosition>& 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<SVzNL3DPosition>& 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; //slope的z变化门限，大于此门限为有限slope
|
||||
int dwnSample = 1;
|
||||
int sampleNum = dataSize / dwnSample;
|
||||
std::vector<SVzNL3DPosition> 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<SSG_basicFeature1D> 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和没有起伏的JUMP和SLOPE
|
||||
{
|
||||
//生成特征
|
||||
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
|
||||
|
||||
/// <summary>
|
||||
/// 提取激光线上的下跳变点（Z值变大的跳变）
|
||||
///
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user