wheelArchHeigthMeasure version 1.4.0 :
添加了标定块凹槽长度计算API。
This commit is contained in:
parent
968fd0910c
commit
bbad1650f2
@ -843,7 +843,7 @@ void wd_getRodArcFeatureGrowingTrees(
|
||||
{
|
||||
for (int i = 0, i_max = (int)all_lineFeatures.size(); i < i_max; i++)
|
||||
{
|
||||
if (i == 1147)
|
||||
if (i == 1304)
|
||||
int kkk = 1;
|
||||
std::vector<SWD_rodArcFeature>& a_lineFeatures = all_lineFeatures[i];
|
||||
for (int j = 0, j_max = (int)a_lineFeatures.size(); j < j_max; j++)
|
||||
|
||||
@ -15,7 +15,8 @@
|
||||
//version 1.3.4 : 轮眉到轮心高度计算方法进行了修正，由Y高度差修改为两点距离
|
||||
//version 1.3.5 : 改进轮眉取点方法。
|
||||
//version 1.3.6 : 轮眉到轮心高度计算方法进行了修正，重新从由两点距离修改为Y高度差。
|
||||
std::string m_strVersion = "1.3.6";
|
||||
//version 1.4.0 : 添加了标定块凹槽长度计算API。
|
||||
std::string m_strVersion = "1.4.0";
|
||||
const char* wd_wheelArchHeigthMeasureVersion(void)
|
||||
{
|
||||
return m_strVersion.c_str();
|
||||
@ -592,6 +593,12 @@ WD_wheelArchInfo wd_wheelArchHeigthMeasure(
|
||||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||||
}
|
||||
|
||||
if(false == isGridData)
|
||||
{
|
||||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||||
return result;
|
||||
}
|
||||
|
||||
//生成水平扫描
|
||||
std::vector<std::vector<SVzNL3DPosition>> hLines_raw;
|
||||
hLines_raw.resize(linePtNum);
|
||||
@ -1044,4 +1051,234 @@ WD_wheelArchInfo wd_wheelArchHeigthMeasure(
|
||||
result.upLine[i] = _ptRotate(result.upLine[i], groundCalibPara.invRMatrix);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
SVzNLRangeD _getClusterZRange(std::vector< SVzNL3DPosition>& a_cluster)
|
||||
{
|
||||
SVzNLRangeD zRange = { -1, -1 };
|
||||
int nodeSize = a_cluster.size();
|
||||
if (nodeSize == 0)
|
||||
return zRange;
|
||||
|
||||
zRange = { a_cluster[0].pt3D.z, a_cluster[0].pt3D.z };
|
||||
for (int i = 1; i < nodeSize; i++)
|
||||
{
|
||||
zRange.min = zRange.min > a_cluster[i].pt3D.z ? a_cluster[i].pt3D.z : zRange.min;
|
||||
zRange.max = zRange.max < a_cluster[i].pt3D.z ? a_cluster[i].pt3D.z : zRange.max;
|
||||
}
|
||||
return zRange;
|
||||
}
|
||||
|
||||
double wd_getCalibrationBlockLength(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const double nominalTotalLen, //标定块总长度(601)，用于过滤虚假信号
|
||||
const SSG_outlierFilterParam filterParam,
|
||||
const SSG_treeGrowParam growParam,
|
||||
const SVzNLRangeD calibratiionBlockYRange, //标定块Y范围
|
||||
#if _OUTPUT_DEBUG_DATA
|
||||
std::vector< std::vector<SVzNL3DPosition>>& debugData,
|
||||
#endif
|
||||
int* errCode
|
||||
)
|
||||
{
|
||||
int lineNum = (int)scanLines.size();
|
||||
int linePtNum = (int)scanLines[0].size();
|
||||
bool isGridData = true;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||||
if (linePtNum != (int)lineData.size())
|
||||
isGridData = false;
|
||||
//滤波，滤除异常点
|
||||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||||
}
|
||||
if (false == isGridData)
|
||||
{
|
||||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||||
return -1;
|
||||
}
|
||||
|
||||
//生成标定块ROI数据
|
||||
#if _OUTPUT_DEBUG_DATA
|
||||
debugData.resize(lineNum);
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
debugData[i].resize(linePtNum);
|
||||
}
|
||||
#endif
|
||||
|
||||
std::vector<SVzNL3DPosition> calibrationData;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
for (int j = 0; j < linePtNum; j++)
|
||||
{
|
||||
SVzNL3DPosition a_pt = scanLines[line][j];
|
||||
a_pt.nPointIdx = (line << 16) | (j & 0xffff);
|
||||
if ((a_pt.pt3D.y < calibratiionBlockYRange.min) || (a_pt.pt3D.y > calibratiionBlockYRange.max))
|
||||
a_pt.pt3D = { 0, 0, 0 };
|
||||
calibrationData.push_back(a_pt);
|
||||
#if _OUTPUT_DEBUG_DATA
|
||||
debugData[line][j] = a_pt;
|
||||
debugData[line][j].nPointIdx = 0;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
//聚类
|
||||
int clusterCheckWin = 5;
|
||||
int clusterDist = sqrt(pow(growParam.zDeviation_max, 2) + pow(growParam.maxSkipDistance, 2) + pow(growParam.yDeviation_max, 2));
|
||||
std::vector<std::vector< SVzNL3DPosition>> objClusters;
|
||||
wd_pointClustering_speedUp(
|
||||
calibrationData,
|
||||
lineNum, linePtNum, clusterCheckWin, //搜索窗口
|
||||
clusterDist,
|
||||
1, //0 - 2d distance; 1- 3d distance
|
||||
objClusters //result
|
||||
);
|
||||
|
||||
//挑选出目标
|
||||
const int minClusterPtSize = 2500;
|
||||
std::vector<int> clusterMask;
|
||||
std::vector<SVzNLRangeD> clusterZRange;
|
||||
clusterMask.resize(objClusters.size());
|
||||
for (int i = 0; i < (int)objClusters.size(); i++)
|
||||
{
|
||||
SVzNLRangeD zRange = _getClusterZRange(objClusters[i]);
|
||||
clusterZRange.push_back(zRange);
|
||||
|
||||
if (objClusters[i].size() < minClusterPtSize)
|
||||
clusterMask[i] = 0;
|
||||
else
|
||||
clusterMask[i] = 1;
|
||||
}
|
||||
|
||||
//将z最小的目标选为标定块
|
||||
int objIdx = -1;
|
||||
for (int i = 0; i < (int)objClusters.size(); i++)
|
||||
{
|
||||
if ( (clusterMask[i] > 0) && (clusterZRange[i].max > 1e-4))
|
||||
{
|
||||
if (objIdx < 0)
|
||||
objIdx = i;
|
||||
else
|
||||
{
|
||||
if( (clusterZRange[objIdx].min + clusterZRange[objIdx].max) > (clusterZRange[i].min + clusterZRange[i].max))
|
||||
objIdx = i;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (objIdx < 0)
|
||||
{
|
||||
*errCode = SG_ERR_ZERO_OBJECTS;
|
||||
return -1;
|
||||
}
|
||||
#if _OUTPUT_DEBUG_DATA
|
||||
for (int i = 0; i < objClusters[objIdx].size(); i++)
|
||||
{
|
||||
int nPtIdx = objClusters[objIdx][i].nPointIdx;
|
||||
int lineIdx = nPtIdx >> 16;
|
||||
int ptIdx = nPtIdx & 0xffff;
|
||||
debugData[lineIdx][ptIdx].nPointIdx = 1;
|
||||
}
|
||||
#endif
|
||||
|
||||
std::vector<std::vector<int>> objMask;
|
||||
objMask.resize(lineNum);
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
objMask[i].resize(linePtNum);
|
||||
std::fill(objMask[i].begin(), objMask[i].end(), -1);
|
||||
}
|
||||
for (int i = 0; i < objClusters[objIdx].size(); i++)
|
||||
{
|
||||
int nPtIdx = objClusters[objIdx][i].nPointIdx;
|
||||
int lineIdx = nPtIdx >> 16;
|
||||
int ptIdx = nPtIdx & 0xffff;
|
||||
objMask[lineIdx][ptIdx] = i;
|
||||
}
|
||||
|
||||
//计算长度
|
||||
std::vector<std::vector<SVzNL3DPosition>> validLines;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
std::vector< SVzNL3DPosition> lineData;
|
||||
for (int j = 0; j < linePtNum; j++)
|
||||
{
|
||||
if (objMask[line][j] >= 0)
|
||||
{
|
||||
int clusterPtIdx = objMask[line][j];
|
||||
if(objClusters[objIdx][clusterPtIdx].pt3D.z > 1e-4)
|
||||
lineData.push_back(objClusters[objIdx][clusterPtIdx]);
|
||||
}
|
||||
}
|
||||
if(lineData.size() > 10)
|
||||
validLines.push_back(lineData);
|
||||
}
|
||||
|
||||
double sumGaugeLen = 0;
|
||||
int sumSize = 0;
|
||||
int validLineNum = (int)validLines.size();
|
||||
for (int i = 0; i < validLineNum; i++)
|
||||
{
|
||||
int validLinePtNum = (int)validLines[i].size();
|
||||
SVzNL3DPosition first_pt = validLines[i][0];
|
||||
SVzNL3DPosition last_pt = validLines[i].back();
|
||||
double totalLen = sqrt(pow(first_pt.pt3D.x - last_pt.pt3D.x, 2) + pow(first_pt.pt3D.y - last_pt.pt3D.y, 2) + pow(first_pt.pt3D.z - last_pt.pt3D.z, 2));
|
||||
double lenDiff = abs(nominalTotalLen - totalLen);
|
||||
if ( (lenDiff < 10) &&(validLinePtNum>100))
|
||||
{
|
||||
//计算凹槽长度
|
||||
double _a = 0, _b = 0, _c = 0;
|
||||
compute2ptLine_2(first_pt.pt3D.y, first_pt.pt3D.z, last_pt.pt3D.y, last_pt.pt3D.z, &_a, &_b, &_c);
|
||||
|
||||
SVzNL3DPosition peakPt_1 = { 0, 0, 0 };
|
||||
double maxLen_1 = -1;
|
||||
for (int j = 0; j < validLinePtNum / 2; j++)
|
||||
{
|
||||
double len = computePtDistToLine(validLines[i][j].pt3D.y, validLines[i][j].pt3D.z, _a, _b, _c);
|
||||
if (maxLen_1 < len)
|
||||
{
|
||||
peakPt_1 = validLines[i][j];
|
||||
maxLen_1 = len;
|
||||
}
|
||||
}
|
||||
|
||||
SVzNL3DPosition peakPt_2 = { 0, 0, 0 };
|
||||
double maxLen_2 = -1;
|
||||
for (int j = validLinePtNum / 2; j < validLinePtNum; j++)
|
||||
{
|
||||
double len = computePtDistToLine(validLines[i][j].pt3D.y, validLines[i][j].pt3D.z, _a, _b, _c);
|
||||
if (maxLen_2 < len)
|
||||
{
|
||||
peakPt_2 = validLines[i][j];
|
||||
maxLen_2 = len;
|
||||
}
|
||||
}
|
||||
|
||||
if ((maxLen_1 > 2.0) && (maxLen_2 > 2.0))
|
||||
{
|
||||
double vLen = sqrt(pow(peakPt_1.pt3D.x - peakPt_2.pt3D.x, 2) + pow(peakPt_1.pt3D.y - peakPt_2.pt3D.y, 2) + pow(peakPt_1.pt3D.z - peakPt_2.pt3D.z, 2));
|
||||
sumGaugeLen += vLen;
|
||||
sumSize++;
|
||||
#if _OUTPUT_DEBUG_DATA
|
||||
int nPtIdx_1 = peakPt_1.nPointIdx;
|
||||
int lineIdx_1 = nPtIdx_1 >> 16;
|
||||
int ptIdx_1 = nPtIdx_1 & 0xffff;
|
||||
debugData[lineIdx_1][ptIdx_1].nPointIdx = 2;
|
||||
int nPtIdx_2 = peakPt_2.nPointIdx;
|
||||
int lineIdx_2 = nPtIdx_2 >> 16;
|
||||
int ptIdx_2 = nPtIdx_2 & 0xffff;
|
||||
debugData[lineIdx_2][ptIdx_2].nPointIdx = 2;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
}
|
||||
if (sumSize == 0)
|
||||
{
|
||||
*errCode = SX_ERR_ZERO_OBJECTS;
|
||||
return -1;
|
||||
}
|
||||
|
||||
sumGaugeLen = sumGaugeLen / sumSize;
|
||||
return sumGaugeLen;
|
||||
}
|
||||
@ -47,3 +47,16 @@ SG_APISHARED_EXPORT WD_wheelArchInfo wd_wheelArchHeigthMeasure(
|
||||
const SSG_treeGrowParam growParam,
|
||||
const SSG_planeCalibPara groundCalibPara,
|
||||
int* errCode);
|
||||
|
||||
//标定块凹槽长度计算
|
||||
SG_APISHARED_EXPORT double wd_getCalibrationBlockLength(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const double nominalTotalLen, //标定块总长度(601)，用于过滤虚假信号
|
||||
const SSG_outlierFilterParam filterParam,
|
||||
const SSG_treeGrowParam growParam,
|
||||
const SVzNLRangeD calibratiionBlockYRange,
|
||||
#if _OUTPUT_DEBUG_DATA
|
||||
std::vector< std::vector<SVzNL3DPosition>>& debugData,
|
||||
#endif
|
||||
int* errCode
|
||||
);
|
||||
@ -1065,6 +1065,93 @@ void _outputScanDataFile_RGBD_obj(char* fileName, std::vector<std::vector< SVzNL
|
||||
sw.close();
|
||||
}
|
||||
|
||||
void _outputScanDataFile_RGBD(char* fileName, std::vector<std::vector< SVzNL3DPosition>>& scanData,
|
||||
float lineV, int maxTimeStamp, int clockPerSecond)
|
||||
{
|
||||
int lineNum = (int)scanData.size();
|
||||
std::ofstream sw(fileName);
|
||||
int realLines = lineNum;
|
||||
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;
|
||||
double alpha = 0.8;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
int nPositionCnt = (int)scanData[line].size();
|
||||
sw << "Line_" << line << "_0_" << nPositionCnt << std::endl;
|
||||
for (int i = 0; i < nPositionCnt; i++)
|
||||
{
|
||||
SVzNL3DPosition& pt3D = scanData[line][i];
|
||||
int type = pt3D.nPointIdx;
|
||||
if (1 == type)
|
||||
{
|
||||
rgb = { 0,255,0 };
|
||||
rgb.r = (int)((double)rgb.r * alpha);
|
||||
rgb.g = (int)((double)rgb.g * alpha);
|
||||
rgb.b = (int)((double)rgb.b * alpha);
|
||||
size = 2;
|
||||
}
|
||||
else if (2 == type)
|
||||
{
|
||||
rgb = { 255,0,0 };
|
||||
rgb.r = (int)((double)rgb.r * alpha);
|
||||
rgb.g = (int)((double)rgb.g * alpha);
|
||||
rgb.b = (int)((double)rgb.b * alpha);
|
||||
size = 4;
|
||||
}
|
||||
else if (3 == type) //轮眉
|
||||
{
|
||||
rgb = { 0, 0, 250 };
|
||||
size = 3;
|
||||
}
|
||||
else if (4 == type) //
|
||||
{
|
||||
rgb = { 255, 255, 0 };
|
||||
size = 3;
|
||||
}
|
||||
else if (5 == type) //
|
||||
{
|
||||
rgb = { 255, 255, 0 };
|
||||
size = 3;
|
||||
}
|
||||
else
|
||||
{
|
||||
rgb = { 100, 100, 100 };
|
||||
size = 1;
|
||||
}
|
||||
|
||||
float x = (float)pt3D.pt3D.x;
|
||||
float y = (float)pt3D.pt3D.y;
|
||||
float z = (float)pt3D.pt3D.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
}
|
||||
sw.close();
|
||||
}
|
||||
|
||||
void _outputScanDataFile_obj_vector(char* fileName, std::vector<std::vector< SVzNL3DPosition>>scanData,
|
||||
std::vector<SSG_peakOrienRgnInfo>&objOps)
|
||||
{
|
||||
@ -2643,10 +2730,11 @@ void _outputScanDataFile_removeZeros(char* fileName, SVzNL3DLaserLine * scanData
|
||||
}
|
||||
|
||||
#define TEST_CONVERT_TO_GRID 0
|
||||
#define TEST_COMPUTE_WHEEL_ARCH 1
|
||||
#define TEST_COMPUTE_WHEEL_ARCH 0
|
||||
#define TEST_CAMERA_ADJUSTING 1
|
||||
#define TEST_COMPUTE_CALIB_PARA 0
|
||||
|
||||
#define TEST_GROUP 6
|
||||
#define TEST_GROUP 7
|
||||
int main()
|
||||
{
|
||||
#if TEST_CONVERT_TO_GRID
|
||||
@ -2690,7 +2778,8 @@ int main()
|
||||
|
||||
#if TEST_COMPUTE_CALIB_PARA
|
||||
char _calib_datafile[256];
|
||||
sprintf_s(_calib_datafile, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据/ground_LaserData.txt");
|
||||
|
||||
sprintf_s(_calib_datafile, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/部署2调平项目_板高3mm/右后离地4mm.txt");
|
||||
|
||||
std::vector< std::vector<SVzNL3DPosition>> scanLines;
|
||||
vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanLines);
|
||||
@ -2711,10 +2800,10 @@ int main()
|
||||
#endif
|
||||
//
|
||||
char calibFile[250];
|
||||
sprintf_s(calibFile, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据/ground_calib_para.txt");
|
||||
sprintf_s(calibFile, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/部署2调平项目_板高3mm/右后离地4mm_ground_calib_para.txt");
|
||||
_outputCalibPara(calibFile, calibPara);
|
||||
char _out_file[256];
|
||||
sprintf_s(_out_file, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据/LaserLine_grund_calib.txt");
|
||||
sprintf_s(_out_file, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/部署2调平项目_板高3mm/右后离地4mm_ground_calib.txt");
|
||||
_outputScanDataFile_self(_out_file, scanLines, 0, 0, 0);
|
||||
printf("%s: calib done!\n", _calib_datafile);
|
||||
}
|
||||
@ -2725,14 +2814,16 @@ int main()
|
||||
const char* dataPath[TEST_GROUP] = {
|
||||
"F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/现场数据/", //0
|
||||
"F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/部署现场数据/", //1
|
||||
"F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/部署现场数据2/11/", //2
|
||||
"F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/部署现场数据2/13/", //3
|
||||
"F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/部署现场数据2/15/", //4
|
||||
"F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/部署现场数据2/17/", //5
|
||||
"F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/部署现场数据2/11_C573/", //2
|
||||
"F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/部署现场数据2/13_C578/", //3
|
||||
"F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/部署现场数据2/15_C552/", //4
|
||||
"F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/部署现场数据2/17_C586/", //5
|
||||
"F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/部署现场2数据/", //6
|
||||
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[TEST_GROUP] = {
|
||||
{1,2}, {1,4}, {11,15}, {11,15}, {11,15},{16,16}
|
||||
{1,2}, {1,4}, {27,41}, {27,41}, {27,41},{27,41}, {7,7}
|
||||
};
|
||||
|
||||
SSG_planeCalibPara poseCalibPara;
|
||||
@ -2756,15 +2847,18 @@ int main()
|
||||
char _scan_file[256];
|
||||
|
||||
int endGroup = TEST_GROUP - 1;
|
||||
for (int grp = 5; grp <= endGroup; grp++)
|
||||
for (int grp = 6; grp <= endGroup; grp++)
|
||||
{
|
||||
char calibFile[250];
|
||||
sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]);
|
||||
poseCalibPara = _readCalibPara(calibFile);
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
//fidx = 6;
|
||||
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
|
||||
//fidx = 2;
|
||||
if(6 == grp)
|
||||
sprintf_s(_scan_file, "%s%d_LaserData_Jl26C575.txt", dataPath[grp], fidx);
|
||||
else
|
||||
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanData;
|
||||
vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanData);
|
||||
if (scanData.size() == 0)
|
||||
@ -2775,7 +2869,8 @@ int main()
|
||||
|
||||
long t1 = GetTickCount64();
|
||||
|
||||
SVzNL3DRangeD wheelRoi3d = { {-1000, 1000}, {-2000, 2000}, {500, 2500} };
|
||||
//SVzNL3DRangeD wheelRoi3d = { {-1000, 1000}, {-2000, 2000}, {500, 2500} };
|
||||
SVzNL3DRangeD wheelRoi3d = { {-1000, 1000}, {-1000, 1000}, {-1000, 1000} };
|
||||
SSG_cornerParam cornerParam;
|
||||
cornerParam.cornerTh = 45; //45度角
|
||||
cornerParam.scale = 50; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
|
||||
@ -2841,7 +2936,11 @@ int main()
|
||||
sprintf_s(_dbg_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx);
|
||||
_outputScanDataFile_RGBD_obj(_dbg_file, scanData, 0, 0, 0, wheelArcHeight);
|
||||
#endif
|
||||
printf("%s: arcY=%f, height=%f, arcToGrund=%f, time=%d(ms)!\n", _scan_file, wheelArcHeight.wheelArchPos.y, wheelArcHeight.archToCenterHeigth, wheelArcHeight.archToGroundHeigth, (int)(t2 - t1));
|
||||
printf("%s: arcY=%f, upPtY=%f, height=%f, arcToGrund=%f, time=%d(ms)!\n", _scan_file, wheelArcHeight.wheelArchPos.y, wheelArcHeight.wheelUpPos.y, wheelArcHeight.archToCenterHeigth, wheelArcHeight.archToGroundHeigth, (int)(t2 - t1));
|
||||
double len = sqrt(pow(wheelArcHeight.wheelUpPos.x - wheelArcHeight.wheelDownPos.x, 2) +
|
||||
pow(wheelArcHeight.wheelUpPos.y - wheelArcHeight.wheelDownPos.y, 2) +
|
||||
pow(wheelArcHeight.wheelUpPos.z - wheelArcHeight.wheelDownPos.z, 2));
|
||||
printf(" 轮毂尺寸：%g\n", len);
|
||||
}
|
||||
else
|
||||
{
|
||||
@ -2851,6 +2950,89 @@ int main()
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if TEST_CAMERA_ADJUSTING
|
||||
const char* dataPath[4] = {
|
||||
"F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/工具标定/11标定/", //0
|
||||
"F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/工具标定/13标定/", //1
|
||||
"F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/工具标定/15标定/", //2
|
||||
"F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/工具标定/17标定/", //3
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[4] = {
|
||||
{1,20}, {1,20}, {1,20}, {1,21}
|
||||
};
|
||||
|
||||
const char* ver = wd_wheelArchHeigthMeasureVersion();
|
||||
printf("ver:%s\n", ver);
|
||||
|
||||
char _scan_file[256];
|
||||
|
||||
for (int grp = 0; grp <= 3; grp++)
|
||||
{
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
//fidx = 6;
|
||||
if(0 == grp)
|
||||
sprintf_s(_scan_file, "%s%d_LaserData_JL26C573.txt", dataPath[grp], fidx);
|
||||
else if (1 == grp)
|
||||
sprintf_s(_scan_file, "%s%d_LaserData_JL26C578.txt", dataPath[grp], fidx);
|
||||
else if (2 == grp)
|
||||
sprintf_s(_scan_file, "%s%d_LaserData_Jl26C552.txt", dataPath[grp], fidx);
|
||||
else
|
||||
sprintf_s(_scan_file, "%s%d_LaserData_JL26C586.txt", dataPath[grp], fidx);
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanData;
|
||||
vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanData);
|
||||
if (scanData.size() == 0)
|
||||
continue;
|
||||
|
||||
long t1 = GetTickCount64();
|
||||
|
||||
double nominalTotalLen = 601.0; //标定块标称总长601mm
|
||||
|
||||
SSG_outlierFilterParam filterParam;
|
||||
filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声
|
||||
filterParam.outlierTh = 5;
|
||||
|
||||
SSG_treeGrowParam growParam;
|
||||
growParam.maxLineSkipNum = 10;
|
||||
growParam.yDeviation_max = 20.0;
|
||||
growParam.maxSkipDistance = 10.0;
|
||||
growParam.zDeviation_max = 20.0;//
|
||||
growParam.minLTypeTreeLen = 100; //mm
|
||||
growParam.minVTypeTreeLen = 100; //mm
|
||||
SVzNLRangeD calibratiionBlockYRange = { -1000, 500 };
|
||||
|
||||
int errCode = 0;
|
||||
#if _OUTPUT_DEBUG_DATA
|
||||
std::vector< std::vector<SVzNL3DPosition>> debugData;
|
||||
#endif
|
||||
double len = wd_getCalibrationBlockLength(
|
||||
scanData,
|
||||
nominalTotalLen,
|
||||
filterParam,
|
||||
growParam,
|
||||
calibratiionBlockYRange,
|
||||
#if _OUTPUT_DEBUG_DATA
|
||||
debugData,
|
||||
#endif
|
||||
& errCode
|
||||
);
|
||||
|
||||
long t2 = GetTickCount64();
|
||||
|
||||
char _dbg_file[256];
|
||||
#if _OUTPUT_DEBUG_DATA
|
||||
|
||||
sprintf_s(_dbg_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx);
|
||||
_outputScanDataFile_RGBD(_dbg_file, debugData, 0, 0, 0);
|
||||
#endif
|
||||
printf("%s: 凹槽间长度=%f, time=%d(ms), errCode=%d\n", _scan_file, len, (int)(t2 - t1), errCode);
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
printf("all done!\n");
|
||||
}
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user